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I. INTRODUCTION 



The study of robotics is a fairly new discipline. 
Although the roots of these studies and developments can be 
traced back to the 1940’ s, the first commercial computer 
controlled robot was not introduced until the late 1950 's 
[Ref. 13 . Furthermore, as the theory developed, several 
common problemmatical methods have been widely accepted and 
used . 

When robot motion is studied, it is usually divided 
into two parts: robot arm dynamics and robot arm kinematics. 
While the kinematics problem deals with the geometry of the 
arm links, the dynamics problem deals with the study of 
forced motion. The dynamics problem is further divided into 
two parts: the direct dynamics problem and the inverse 
dynamics problem. In the inverse dynamics problem, link 
variables such as acceleration and velocity are known and 
the forces and necessary joint torques for the desired 
motion are calculated. In the direct dynamics problem, the 
joint torques are known and the accelerations and velocities 
of each joint are calculated. 

The kinematics problem is also divided into two parts: 
the direct kinematics problem and the inverse kinematics 
problem. The direct kinematics problem is, given a set of 
critical geometric joint and link variables for each of the 
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joint-link pairs and the joint angle vector, determine the 
position and orientation of the end effector of the 
manipulator. The Denavit-Hartenberg representation, which 
uses a homogeneous transformation matrix to describe the 
spatial relationships between two adjacent rigid mechanical 
links is the most common method used to study the direct 
kinematics problem [Ref. 2]. The inverse kinematics problem 
is, given a desired position and orientation of the end 
effector of the manipulator and a set of critical geometric 
joint and link parameters, determine the corresponding joint 
angle vector; i.e., find all of the joint angles of the 
robot arm so that the end effector can be positioned in the 
desired location. 

A difficulty in the solution to the inverse kinematics 
problem arises when two successive links align [Ref. 3]. At 
these times the angle between two successive links becomes 0 
or 180 degrees, and the Jacobian matrix which relates the 
end effector motion to the joint variable variations cannot 
be inverted. This means that motion cannot be simulated. 
Different approaches to this problem have been investigated. 
One method deals with the Newton-Euler approach with a 
moving coordinate system [Refs. 4, 5], another uses the 
Langrangian approach [Refs. 6, 7]. One method deals with a 
virtual work approach [Ref. 8]. Kane’s dynamics equations 
have been used due to computational efficiency [Ref. 9]. 
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However, none of these methods have been able to overcome 
this singularity problem [Ref. 3]. 

Several methods have been proposed to avoid the 
singular configuration. One method proposed to minimize the 
time near the singular points [Ref. 10], thereby reducing 
their effects. In another method, it was proposed to avoid 
these singular points by confining the motion [Ref. 11]. 
Other solutions deal with presenting equations that can 
translate the manipulator in the neighborhood of a 
singularity through identification of singular points 
beforehand [Refs. 12, 13, 14]. It has also been shown that 
the redundancy of robot manipulators is effective in dealing 
with the singularities [Refs. 15, 16, 17]. 

In this thesis the equations of motion are derived 
using the principles of Newtonian dynamics in terms of a 
globally fixed coordinate system to overcome the singularity 
problem. Each link is treated as a free body with forces 
and moments applied at the joints and free body analysis is 
used to derive the equations of motion. Although the 
equations are relatively long and the solution to the 
problem is computationally time consuming, it is shown that 
these equations do overcome the singularity problem. The 
direct dynamics and the inverse dynamics problem are both 
simulated. 
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II. THEORETICAL DEVELOPMENT 



A. THEORY OF THE SOLUTION 

To derive the non-singular equations of motion the 
Newton-Euler approach is used (Figure 1). Each link is 
treated as a free body with forces and moments applied to 
it, weight has been disregarded. The globally fixed X Y Z 
coordinate system is used for the equations. All links are 
assumed to be rigid, so the effects of flexibility are not 
considered. All of the distances and the directions of the 
forces and moments have been based on the fixed coordinate 
system rather than a local coordinate system which moves 
with the link [Refs. 4, 5]. The link masses, the initial 
link positions and the orientations are assumed to be known 
parameters. As a result of equation derivation in the fixed 
reference frame, the moment of inertia is allowed to change 
with respect to time and is calculated for each small 
integration interval. This is opposed to keeping inertia 
constant as used in the local coordinate formulations. But 
it is assumed that the moment of inertia is constant in each 
small integration interval. This last assumption 
effectively linearizes the equations of motion so that a 
non-singular matrix inversion can be used to solve the 
equation set. 
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FZl 




Figure 1. Free Body Diagram of a Three Link Manipulator 
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To calculate the moment of inertia in each integration 
interval, the link direction cosine angles with respect to 
the fixed coordinate system were used. Acceleration of 
joint zero was input as zero. For each link the three 
linear acceleration components, three angular acceleration 
components and forces at each joint were considered to be 
the unknown variables. Based on the Newtonian dynamics and 
the manipulator kinematics [Ref. 18], the equations were 
derived as follows: 

B. DYNAMIC EQUATIONS OF MOTION OF LINK ONE 
1 . Sum of Forces Equations 

In the free body analysis of link one (Figure 1) the 
sum of the forces in the x direction is: 

2Fx = Fxl - FxO = Mlaxl (1) 

Similarly sum of the forces in the y direction is: 

2Fy = Fyl - FyO = Mlayl (2) 

and the sum of the forces in the z direction is: 

ZFz = Fzl - FzO - W1 = Mlazl (3) 
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2 . Joint Equations 



We begin by evaluating the joint equations at joint 
zero [Ref. 19, equation (8/4), pp. 423]. If the joint is 
sequested and analysis conducted at a point on link zero 
(subscript a) and another at a point on link one (subscript 
b) that is common to both, so when linked together they are 
equal. This results in two equations and the two unknowns 
wdl and wl . As a result: 

Aa = Ao 

which is the acceleration at joint zero, and 

Ab = A1 + (wdl X rb/Gl) + wl x (wl x rb/Gl) 

which is the acceleration of point b on joint one. Here 
rb/Gl is the distance from point b to the center of gravity 
of link one, and A1 is the acceleration at the center of 
mass of link one or, 

rb/Gl = ( jxO-LCOGxl ) i + ( jyO-LCOGyl ) j + ( jzO-LCOGzl )k 
= rb/Glx + rb/Gly + rb/Gl z 

After equating Aa and Ab and having the known variables on 
the right side of the equation and unknown variables on the 
left side the following sets of equations result: 
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Axl + wdyl ( rb/Glz ) - wdzl(rb/Gly) = Aox - MICO 



(4) 



where MICO equals 

= wylwxl ( rb/Gly ) - ( wyl ) 2 ( rb/Glx) - (wzl ) 2 ( rb/Glx) 
+ wzlwxl ( rb/Glz ) 

also 



Ayl + wdzl( rb/Glx) - wdxl( rb/Glz) = Aoy - MJCO (5) 
where MJCO equals 

= wzlwyl ( rb/Glz ) - ( wzl )2(rb/Gly ) - ( wxl ) 2 ( rb/Gly ) 

+ wxlwyl ( rb/Glx) 

and 



Azl + wdxl( rb/Gly) - wdyl (rb/Glx) = Aoz - MKCO (6) 
MKCO equals 

= wxlwzl(rb/Glx) - (wzl )2(rb/Glz) - (wyl )2(rb/Glz) 

+ wylwzl ( rb/Gly ) 

3 . Sum of Moment Equations 

Computing the sum of the moment equations about the 
center of gravity results in: 
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2M1 



(rO/Gl X FO) + (rl/Gl x FI) - T1 + TO 



where the vector rO/Gl is the distance from joint zero to 
the center of gravity of link one and vector rl/Gl is the 
distance from joint one to the center of gravity of link 
one, in the x, y and z directions. Such that 
rO/Gl = rjO - rGl 

and 

rl/Gl = rjl - rGl 



so 

rjO - rGl = (xjO - xGl)i + (yjO - yGl)j + (zjO - zGl)k 

and 

rjl - rGl = (xjl - xGl)i + (yjl - yGl)j + (zjl - zGl)k 
In the X, y and z directions the sum of moment equations 
are : 

2M1 in X direction = 

(yjO/Gl)FzO + (zjO/Gl)FyO + (yjl/Gl)Fzl - (zjl/Gl)Fyl 
- Tlx + TOx (7a) 

2M1 in y direction= 

(zjO/Gl)FxO + (xjO/Gl)FzO + (zjl/Gl)Fxl - (xjl/Gl)Fzl 
-Tly + TOy (8a) 
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2M1 in z direction= 



(xjO/Gl)FyO + (yjO/Gl)FxO + (xjl/Gl)Fyl - (yjl/Gl)Fxl 
-Tlz + TOz (9a) 

From [Ref. 19, equation (57), pp. 227] the sum of the moments 
about a fixed point that does not move with the body is 
equal to the time rate of change of angular momentum of the 
system (H) about the fixed point, 2M = H. In the present 
study we have let each link be a composite body of two 
elements. The angular momentum (H) for a composite body 
where the number of elements of the body is two, about the 
center of gravity of each link is Hi -2 [Ri x (w x 

Ri)]Mi, where Ri is the distance from the center of gravity 
of each link to the appropriate element (1 or 2) in the x, y 
and z direction. So: 

2 [Ryi(wx(Ryi) - wy(Rxi)) - Rzi(wz(Rxi)- 

wx(Rzi ) ) ]Mi 

[R2yl(wx) - Ryl(Rxl)(wy) - Rzi(Rxl)(wz) + 
R2zl(wx)]Ml + [R2y2(wx) - Ry2(Rx2 ) ( wy ) - 

Rz2(Rx2)(wz) + (R2z2)wx]M2 

If I XX = Ry2 + Rz2 dm, 
and Ixy = RxRy dm, 
and Ixz = RxRz dm. 



Hx = 
Hx = 
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then: 



Hx = [Ilxx(wx) - rixy(wy) - Ilx 2 (w 2 )]Ml 

+ [I2xx(wx) - I2xy(wy) - I 2 x 2 (w 2 )] M2 

and 

HDx = [Ilxx(wdx) - Ilxy(wdy) - Ilx 2 (wd 2 )]Ml 

+ [I2xx(wdx) - I2xy(wdy) - I2x2(wd2)]M2 (7b) 

by assuming the moment of inertia changes with time but is 
constant for a given time interval. 

By similar analysis it can be shown: 

Hy = 2 [R 2 i(wy(R 2 i) - ws(Ryi)) - Rxi(wx(Ryi)- 

wy(Rxi) - wy(Rxi))]Mi 
and if lyy = Rx2 + Rz2 dm, 
and Iy 2 = RyRs dm, 

and Ixy = RxRy dm, 

then: 

HDy = Cllyy(wdy) - Hy 2 (wd 2 ) - Ily 2 (wdx)]Ml 

+ [I2yy(wdy) - I2y2(wd2) - I2yx(wdx)]M2 (8b) 

and 

H 2 = 2 [ Rxi(w 2 (Rxi) - wx(Rsi)) - Ryi(wy(R 2 i)- 

W 2 (Ryi ) ) ]Mi 

if I 22 = Rx2 + Ry2 dm. 

So H 2 = [Il 22 (w 2 ) - Ily 2 (wy) - Il 2 x(wx)]Ml 

+ [I222(w2) - I2y2(wy) - I22x(wx)]M2 
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then 

HDz = [Ilzz(wdz) - Ilyz(wdy) - Ilzx(wdx)]Ml 

+[I2zz(wdz) - I2yz(wdy) - I2zx(wdx)]M2 (9b) 

Combining equations (7a) and (7b) and keeping known 
variables on the right side and unknown variables on the 
left side yields: 

2Mlx = (-yjO/Gl)FzO + (zjO/Gl)FyO + (yjl/Gl)Fzl 

- (zjl/Gl)Fyl - HDx = Tlx - TOx (7) 

Combining equations (8a) and (8b) yields: 



2Mly = (-zjO/Gl)FxO + (xjO/Gl)FzO + (zjl/Gl)Fxl 

- (xjl/Gl)Fzl - HDy = Tly - TOy (8) 

Combining equations (9a) and (9b) yields: 

2Mlz = -(xjO/Gl)FyO + (yjO/Gl)FxO + (xjl/Gl)Fyl 

- (yjl/Gl)Fxl - HDz = Tlz - TOz (9) 

C. LINK TWO EQUATIONS 

1 . Sum of Forces Equations 

From the free body diagram (Figure 1) it follows 

that 
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2Fx = Fx2 - Fxl = M2ax2 



( 10 ) 



SFy = Fy2 - Fyl = M2ay2 



( 11 ) 



2Fz = Fz2 - Fzl = M2az2 



(12) 



2 . Joint Equations 

Analysis is conducted at joint one where similar 
equations are used as in joint zero with a point on link one 
(a) and one on link two (b). For point a the equation is 

Aa = A1 + wdl X ra/Gl + wl x (wl x ra/Gl) 

ra/Gl is a vector whose distance is measured from point a to 
the center of gravity of link one in the x, y and z 
direction . 

ra/Gl = (jxl - LCOGxDi + (jyl - LC0Gyl)j 
+ (jzl - LC0Gzl)k 
= ra/Glx + ra/Gly + ra/Glz 

For point b the equation is: 

Ab = A2 + wd2 x rb/G2 + w2 x (w2 x rb/G2) 
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where rb/G2 is a vector whose distance is measured from 
point b to the center of gravity of link two. 

rb/G2 = (jxl - LC0Gx2)i + (jyl - LC0Gy2)j 
+ (jzl - LC0Gz2)k 
= rb/G2x + rb/G2y + rb/G2z 

Equating Aa and Ab and setting knowns and unknowns 
on the respective sides of the equation results in: 



Ax2 - Axl + wdy2(rb/G2z) - wdz2(rb/G2y) - wdyl(ra/Glz) 

+ wdzl(ra/Gly) = MICl - M1C2 (13) 

MICl = wylwxl ( ra/Gly ) - (wyl)2(ra/Glx) - ( wzl ) 2 ( ra/Glx) 
+ wzlwxl (ra/Glz) 



MIC2 = wy2wx2(rb/G2y) - ( wy2 ) 2 ( rb/G2x) - (wz2 ) 2 ( rb/G2x) 
+ wz2wx2 ( rb/G2z ) 

Ay2 - Ayl + wdz2(rb/G2x - wdx2(rb/G2z) - wdzl (ra/Glx) 

+ wdxK ra/Glz) = MJCl - MJC2 (14) 



MJCl = wzlwyl ( ra/Glz ) - (wzl ) 2( ra/Gly ) - (wxl ) 2 ( ra/Gly ) 
+ wxlwyl ( ra/Glx) 
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MJC2 = wz2wy2 ( rb/G2z ) - ( wz2 ) 2 ( rb/G2y ) - (wx2)2(rb/G2y) 



+ wx2wy2( rb/G2x) 



AZ2 - AZl + wdx2(rb/G2y) - wdy2(rb/G2x) - wdxl(ra/Gly) 
+ wdyl(ra/Glx) = MKCl - MKC2 (15) 

MKCl = wxlwzl ( ra/Glx) - (wxl ) 2 ( ra/Glz ) - (wyl )2(ra/Glz) 
+ wylwzK ra/Gly ) 

MKC2 = wx2wz2 ( rb/G2x) - (wx2) 2 ( rb/G2z ) - (wy2)2(rb/G2z) 
+ wy2wz2( rb/G2y ) 

3 . Sum of the Moment Equations 

These equations have a similar development as that 
of link one: 



2M2 = (rjl/G2) x FI + ( 

where 



rjl/G2 


= 


(xjl 


- xG2 


)i + 


rj2/G2 


= 


(xj2 


- xG2 


)i + 


2M2x = 


- 


(yjl 


- yG2 


)Fzl 




+ 


(yj2 


- yG2 


)Fz2 




+ 


Tlx 


- T2x 





j2/G2) X F2 + T1 - T2 



(yjl ■ yG2)j + (zjl - zG2)k 
(yj2 - yG2)j + (zj2 - zG2)k 
+ (zjl - zG2)Fyl 
- (zj2 - zG2)Fy2 

(16a) 
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ZM2y = - (zjl - zG2)Fxl + (xjl - xG2)Fzl 

+ (zj2 - zG2)Fx2 - (xj2 - xG2)Fz2 

+ Tly - T2y (17a) 

ZM2z = - (xjl - xG2)Fyl + (yjl - yG2)Fxl 

+ (xj2 - xG2)Fy2 - (yj2 - yG2)Fx2 

+ Tlz - T2z (18a) 

From angular momentum equation developed for link one, it 
can be shown for link two: 



ZM2x 


= HDx 


(16b) 


2M2y 


= HDy 


(17b) 


2M2z 


= HDz 


(18b) 



Combining equations (16a) and (16b) the following 

result : 

- (yjl - yG2)Fzl + (zjl - zG2)Fyl + (yj2 - yG2)Fz2 

- (zj2 - zG2)Fy2 - HDx = - Tlx + T2x (16) 

Combining equations (17a) and (17b) yield the 
following result: 

- (zjl - zG2)Fxl + (xjl - xG2)Fzl + (zj2 - zG2)Fx2 

- (xj2 - xG2)Fz2 - HDy = -Tly + T2y ' (17) 
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Combining equations (18a) and (18b) yield the 
following result: 

- (xjl - xG2)Fyl + (yjl - yG2)Fxl + (xj2 - xG2)Fy2 

- (yj2 - yG2)Fx2 - HDz = -Tlz + T2z (18) 

D. LINK THREE EQUATIONS 

1 . Sum of Forces Equations 

Following similar logic from that previously 

developed: 

2Fx = - Fx2 = M3ax3 (19) 

2Fy = - Fy2 = M3ay3 (20) 

2Fz = - Fz2 - W3 = M3az3 (21) 

2 . Joint Equations 

With point a on link two and point b on link three 
one gets for joint equations at joint two: 

Aa = A2 + (wd2 x ra/G2) + w2 x (w2 x ra/G2) 

where ra/G2 is a vector whose distance is measured from 
point a to center of gravity of link two in the x, y and z 
direction . 
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ra/G2 = ( jx2 - LC0Gx2)i + (jy2 - LC0Gy2)j 



+ (jz2 - LC0Gz2)k 
= ra/G2x + ra/G2y + ra/G2z 



For point b 

Ab = A3 + wd3 x rb/G3 + w3 x (w3 x rb/G3) 

where rb/G3 is a vector whose distance is measured from 
point b to center of gravity of link three in the x, y and z 
direction. 

rb/G3 = ( jx2 - LC0Gx3)i + (jy2 - LC0Gy3)j 
+ (jz2 - LC0Gz3)k 
= rb/G3x + rb/G3y + rb/G3z 

Equating Aa and Ab and setting knowns and unknowns on the 
respective sides of the equation results in: 

Ax3 - Ax2 + wdy3(rb/G3z) - wdz3(rb/G3y) - wdy2(ra/G2z) 

+ wdz2(ra/G2y) = MIC3 - MIC4 (22) 

MIC3 = wy2wx2 ( ra/G2y ) - ( wy2 ) 2 ( ra/G2x) - ( wz2 ) 2 ( ra/G2x) 
+ wz2wx2 (ra/G2z ) 

MIC4 = wy3wx3(rb/G3y) - (wy3)2(rb/G3x) - ( wz3 )2 ( rb/G3x 
+ wz3wx3 ( rb/G3z ) 
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Ay3 - Ay2 + wdz3 ( rb/G3x) - wdx3(rb/G3z) - wdz2(ra/G2x) 

+ wdx2(ra/G2z) = MJC3 - MJC4 (23) 

MJC3 = wz2wy2 ( ra/G2z ) - ( wz2 ) 2 ( ra/G2y) - ( wx2 ) 2 ( ra/G2y ) 
+ wx2wy2(ra/G2x) 

MJC4 = wz3wy3(rb/G3z) - w2z3(rb/G3y) - w2x3(rb/G3y) 

+ wx3wy3 ( rb/G3x) 

AZ3 - AZ2 + wdx3(rb/G3y) - wdy3(rb/G3x) - wdx2(ra/G2y) 

+ wdy2(ra/G2x) = MKC3 - MKC4 (24) 

MKC3 = wx2wz2(ra/G2x) - ( wx2 ) 2 ( ra/G2z ) - ( wy2 ) 2 ( ra/G2z ) 
+ wx2wy2(ra/G2y) 

MKC4 = wx3wz3 ( rb/G3x) - ( wx3 ) 2 ( rb/G3z ) - ( wy3 ) 2 ( rb/G3z ) 
+ wy3wz3 ( rb/G3y ) 

3 . Sum of Moment Equations 

As in the development of the equations for link one: 

2M3 = (rj2/G3) x F2 + T2 

where 

rj2/G3 = (xj2 - xG3)i + (yj2 - yG3)j + (zj2 - zG3)k 
= XJ2/G3 + yj2/G3 + zj2/G3 



43 



2M3x = (-yj2/G3)Fz2 + (zj2/G3)Fy2 + T2x (25a) 

2M3y = (-zj2/G3)Fx2 + (xj2/G3)Fz2 + T2y (26a) 

2M3z = (-xj2/G3)Fy2 + (yj2/G3)Fx2 + T2z (27a) 

From the angular momentum theory: 

2M3x = HDx (25b) 

2M3y = HDy (26b) 

2M3z = HDz (27b) 

Combining equations (25a) and (25b) the following results: 

(-yj2/G3)Fz2 + (zj2/G3)Fy2 - HDx = - T2x (25) 

Combining equations (26a) and (26b) the following results: 
(-zj2/G3)Fx2 + (xj2/G3)Fz2 - HDy = - T2y (26) 

Combining equations (27a) and (27b) the following results: 
(-xj2/G3)Fy2 + (yj2/G3)Fx2 - HDz = - T2z (27) 
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III. COMPUTATIONAL APPROACH 



A. PROGRAM MATRICIES 

The Dynamic Simulation Language (DSL) was used to 
simulate the motion. This computer code was compiled on an 
IBM 3033 computer by using the FORTVS compiler and all of 
the calculations have been done in double precision. The 
entire simulation process is shown in Figure 2 and is 
discussed below. 

The principle program matrix, Matrix A (MATA, 27*27), 
was created from the coefficients of the unknown variables 
in equations 1 to 27. In the simulation of the direct 
dynamics problem, a corresponding 27*1 Matrix B (MatB) was 
generated from all known variables, also from equations 1 to 
27. A subroutine CPROD was used to perform all the cross 
product terms required in the main program. The resulting 
equations are shown in Figure 3, in the final matrix form. 
During a simulation time step, the link inertias, the link 
velocities and the link positions were all assumed constant. 
IMSL subroutine LEQT2F was called to invert the matrix A a,nd 
get the generalized solution x from Ax = B. This subroutine 
uses Gaussian elimination with iterative improvement to get 
a high accuracy solution to the problem. The output from 
LEQT2F then returns as MATB, which contains the solution to 
the equations. The outputs were used by DSL to integrate 
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Figure 2. Computer Simulation Flow Chart 
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the linear and angular accelerations of each link to get the 
linear and angular velocities respectively. The linear 
velocities for each link were next integrated to get the 
linear displacements of center of gravity of each link. 
Although the linear velocities in the fixed reference frame 
can be integrated to get the linear displacements, this idea 
is not true for the angular displacements [Refs. 20, 21]. 

To get the angular displacements, a set of 
transformation matrices must be used on the velocities, then 
the motion can be integrated. That is, the angular 
velocities of each link in the fixed reference system must 
first be converted into equilavences in a body fixed 
coordinate system then into body Euler rates and Euler 
angles to define the motion unambigously . In this thesis, 
the body coordinate velocities are called Bratel, Brate2 and 
BrateS for the link one, link two and link three 
respectively. To convert these velocities into the Euler 
rates, another transformation matrix is used. That is, the 
transformation matrix is multiplied by body rates to get the 
Euler angle rates for each link. These later rates are 
defined as the Yaw rate (about the x axis), the Pitch rate 
(about the y axis) and the Roll rate (about 2 axis). These 
rates are called as Ratel, Rate2 and Rate3 for link one, 
link two and link three. After the transformation of 
velocities to the Euler rates, they can be directly 
integrated to get the Euler angles. In this thesis, these 
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angles are called the Yaw, pitch and Roll angles about the x 
y 2 axis respectively [Refs. 2, 20]. 

This convention is very important and should not be 
mixed with another set of Euler angles described differently 
in the literature [Refs. 3, 20]. In addition to that, the 
order of the rotation must be decided beforehand. This is 
true because the orientation of objects is different when 
they are rotated in a different order, i.e., first the 
rotation about x axis, then a rotation about the y axis, 
finally a rotation about the z axis will produce a different 
orientation in space than the one which was defined and used 
in this thesis (z, y, then x). The transformation matrices 
used here are valid as long as the assumed order of the 
rotation is retained. 

In the literature, a quite different set of angles is 
used to describe the orientation [Refs. 2, 3]. While some 
of these angles define the orientation with respect to a 
non-orthogonal coordinate system some others may define with 



respect to 


an 


orthogonal system. 


Euler 


angles 


define an 


independent 


set 


of coordinates 


system 


which 


are 


not 


orthogonal . 




Therefore, all 


three 


coordinates 


are 


independent 


from 


each other and 


velocities 


in 


this 



coordinate system can be directly integrated to get the 
relevant angles. They describe the unique orientation of 
the body in space. The orthogonal set of coordinate axes do 
not form an independent coordinate system. This is true 
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since the three axis have a certain relation with each other 
in any position, i.e., direction cosine angles have a unique 
relation in a fixed reference system and cannot be obtained 
by integrating any velocity in an orthogonal coordinate 
system. The velocities in an orthogonal coordinate system 
must thus be converted to a nonorthogonal coordinate system 
(e.g. Euler angle rates) prior to integration. 

After Yaw, Pitch and Roll angles are calculated, it is 
possible to go back and express the orientation of the body 
with the direction cosines in an orthogonal coordinate 
system. The columns of the transformation matrix from one 
orthogonal set of axes to another describes the orientation 
of the new coordinate axis with respect to old coordinate 
system. So, a transformation matrix can be used to get the 
direction cosine angles. The direction cosines of each link 
are then used to calculate the moment of inertia of the 
links. The variation of a link inertia with respect to time 
was shown in Figure 4 as it was calculated during a 
simulation run. The derivation of the transformation 
matrices is shown in Appendix A. 

B. CONSTRAINTS IN THE SIMULATION PROGRAM 

In the development of the equations, thus far, each 
link has been treated such that it can move in space without 
any constraint. For most cases, however, degrees of freedom 
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Figure 4. Moment of Inertia 
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TIME (SEC) 



of each link must be reduced so that the link can move only 
in the direction permitted by its joint. 

In the simulation of the direct dynamics problem, the 
base rotation is transmitted to the second and third links 
for the three revolute joint arm which was studied. This 
was simulated by allowing the first link to rotate only 
about Z axis. At the same time, the rotational rates of the 
second and third links about the Z axis were made equal. 

To make any of the simulation variables zero, meaning 
no variation in that direction, one zeroes the related rows 
and columns in MatA putting 1 on the diagonal. At the same 
time, if the same row in MatB is made zero, the 
corresponding mathematical expression for this equation will 
be in the form of 1 * X = 0, and a result of this, X will be 
equal zero. This idea can also be applied to MatA and MatB 
to make two variables equal so that XI - X2 = 0. Thus, the 
above motion was simulated by constraint. 

C. PROGRAM VALIDATION 

The validation of the inverse dynamics problem has been 
conducted in several cases. In this approach the idea was 
to choose an angular acceleration such that at a certain 
time, two of the three links would align. In other words, 
the links would be in a singular position at this time, and 
if the simulation procedure worked, the singularity problem 
would have been avoided. 
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The validation procedure is shown in Figure 5. For 
this procedure link two angle was chosen as 0 = (Pi/2) * 
sin (pi/2) * Time. This time dependent function has a period 
of 4 sec and an amplitude of 90 degrees. The second 
derivative of this function is 0 = - ((pi**3)/8) * sin(pi/2) 
Time and corresponds to the angular acceleration of the 
link. This value was input as the theoretical angular 
acceleration in the simulation program, and corresponding 
linear accelerations and forces at each joint were 
calculated. The other two links were forced to have zero 
rotational velocity throughout the simulation. 

To apply a corresponding torque at the joint, MaTA and 
MatB were multiplied and a right hand side matrix DQ (27^1) 
was obtained (MATA * MATB = DQ) . This matrix DQ ( 27 ^ 1 ) was 
used to solve the simulation equations in the form of 
AX = DQ. The vector X (that is, theta) was fedback in the 
loop and the theoretical and the calculated values of theta 
were compared. 

The above discussion has been implemented in three 
different initials configuration as shown in Figure 6. To 
force the arm links to' the various singular points, several 
different plane motions were simulated. For each 
configuration, three different angular motions were input 
for link 2, or as can be seen from Figure 6, for each 
configuration, one angular velocity caused a spinning motion 
of the link about the axis with which it was initially 
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Figure 5. Validation Procedure Flow Chart 
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Figure 6. 




Initial Orientation of Links for Validation 
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aligned, while the other two produced a plane motion 
according to direction of the applied angular motion. The 
angles between two successive links were measured for each 
motion. Figure 7 shows the angle variation between link 1 
and link 2, and link 2 and link 3 corresponding to an 
angular acceleration applied in the X direction for 
configuration A. As can be seen from Figures 7 and 8, two 
successive links pass through the singular points every 2 
seconds, i.e., they align and the angle between links 
becomes either 0 or 180 degrees. (The singular points are 
marked on the graph) . Figure 8 shows the angle variations 
for an angular motion applied in the Z direction for 
configuration A. In this case, it is obvious that the angle 
between link 1 and link 2 is always constant (90 degrees). 

f 

The second graph on Figure 8 shows the angle between link 2 
and link 3 now, singularity occurs on the Z motion, with the 
singularities marked as in Figure 8. Figure 7 and Figure 8 
are representative of the data obtained in the validation 
procedure which analyzed nine possible motions of link 2 
leading the singularity. This data showed that singularity 
in these directions could be overcome, and a solution to the 
problem exists using this approach. 

For each run, the error between the theoretical and the 
simulated value of Theta was computed. Figure 9 shows the 
percent error for the X motion for configuration A (Figure 7 
Data) . The trend of the error is representative of every 
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(saaaoaa) ai3Mv 

Figure 7. Configuration A, Link 2 Wx Motion 
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Figure 8. Configuration A, Link 2 Wz Motion 
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Figure 9. Percent Error Between Theoretical and 
Simulated Angles 
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2,0 

TIME (SEC) 



case investigated. The figure shows that, due to nature of 
the numerical integration, the error slightly accumlates 
during the simulation, but still has very small value. This 
proves that the direct dynamics problem can be solved very 
accurately by Newton-Euler approach in a fixed coordinate 
system. 
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IV. RESULTS AND RECOMMENDATIONS 



1. A dynamic model of a three link, rigid revolute joint 
manipulator has been developed in this thesis, as a 
general computer program package. 

2. Several runs for different initial configurations were 
simulated and the singularity problem was investigated. 
Theoretical and calculated values of angular positions 
were compared. It was proved that the singularity 
problem could be overcome by using a Newton-Euler 
approach in a fixed coordinate system. 

3. The following recommendations are provided: 

a. Enhance the code and make it more interactive. 
That is, let the user specify the constraints he 
wants to apply on each link by answering 
interactive questions before the actual simulation 
run starts. Thus, the motion can be simulated 
with different constraints without going into the 
code and changing the relevant parameters. 

b. Adapt the code for use in a microcomputer. Add a 
subroutine in the program to invert the matrix A. 
Thus, the code will be more independent from 
outside routines and more adaptable to other 
computer systems . 
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c. Validation of the approach via actual experimental 

tests in crucial. This will establish a way of 

; 

developing accurate constants for subsequent 
controller design and provide a basis for 
compensation of gravity effects. Determining 
these constants for the code will make the 
simulation program more concrete and will provide 
more physical insight. 

d. Finally, develop a controller for a manipulator 
which makes use of the present algorithm for 
validation and design. 
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APPENDIX A 



DERIVATION OF THE TRANSFORMATION MATRIX FROM 
EARTH FIXED COORDINATE SYSTEM TO BODY FIXED COORDINATE SYSTEM 

The angular velocity terms obtained by integration of 
the angular acceleration terms are with respect to an Earth 
fixed coordinate system. To define the Euler angles which 
are called Yaw Pitch and Roll angles in this thesis, we 
have to establish an appropriate body fixed coordinate 
system. Thus, U, V and W is a right hand coordinate system 
[Ref. 20] with its origin fixed at the center of gravity of 
a link. The U, V, W coordinate system is initially oriented 
such that the angles between two coordinate system axes are 
simultaneously reduced to zero, i.e., i, -j, k, axis are 
parellel to the I, J, K respectively. 

If a rotation from X Y Z coordinate system to the U V W 
coordinate system is accomplished by first rotation about K 
axis (roll), then about J axis (pitch) and finally about I 
axis (yaw), it follows that for any arbitrary point in the X 
Y Z coordinate system, the corresponding coordinates in the 
U V W system are; 
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where MatR is a 3*3 matrix. 



To get the transformation matrix, we need to examine each 
rotation separately. 

Rotation about the Z axis; 




a = Xc + Ts 
V = +Yc(() 
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Rotation about the Y 




axis; 
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Rotation about the X axis; 




u. X 

V= YC9 
W.-Ysip+ 




V 



o 





By multiplying three rotation matrix together; 





C0C(J 


ces<j> 


s © 


MATR * 




5^|;ses4+c<|>c(t> 






c^secf + s<jjs<p 


C(^jes<^ - s(jfcf 




where 


c = cos 








S = Sin 








T = Tan 







The transformation matrix from body fixed to Euler 
coordinate system is obtained as below [Ref. 20]. 

■ O Cf 

I T6if Tecji 

0 StcQSf SecBc^ 

The angles discussed above are shown in Figure 10. 
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Figure 10. Critical Angles 

(a) Euler Angles, Body Coordinates 

(b) Direction Cosines, Global Coordinates 
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APPENDIX B 



THREE DIMENSIONAL DIRECT DYNAMICS SIMULATION PROGRAM 



R2(3,2) 

IZZ(3,2) 



TERMINAL 
METHOD ADAMS 

PRINT .05,DRCANX(l-3) ,DRCANY(l-3) ,DRCANZ(l-3), . . . 

JXO, JY0,JZ0,JX1, JYl, JZ1,JX2,JY2,JZ2, . . . 

LCOGX(l-3) ,LCOGY(l-3) ,LCOGZ(l-3) 

CONTROL FINTIM =2.0, DELMAX =.l, DELPRT = .05 
D DIMENSION MATA(27 , 27 ) ,MASS (3 , 2) ,L(3 , 2) , RX(3 , 2) ,RY(3 , 2) , 

D DIMENSION IXX(3 , 2) , IXZ(3 , 2) , IXY(3 , 2) , IYY(3 , 2) , IYZ(3 , 2) , 

D DIMENSION MAT1R(3 ,3) ,MAT2R(3 ,3) ,MAT3R(3 ,3) 

D DIMENSION MATIT (3 , 3 ) ,MAT2T(3 , 3 ) ,MAT3T (3 , 3 ) 

D INTEGER lER , I , J ,M,K, P,N, lA , IDGT , A 
EXCLUDE IA,IDGT,IER,I, J,M,K,P,N,A 
ARRAY MATB(27),LCOGX(3) ,LCOGY(3) ,LC0GZ(3) 

ARRAY VECTA0(3) , VECTBO(3 ) , VECTAl (3 ) , VECTBl (3 ) , VECTA2(3 ) ,VECTB2(3 ) 

ARRAY WDX(3),WDY(3),WDZ(3Kw1(3; 

ARRAY RATElU) .RATE2(3) ,RATE3(3; 

ARRAY RBG1(3) ,RAG1(3) ,RBG2(3) ,RAG2(3) ,RBG3(3) 

ARRAY SUMHDX(3) ,SUIfflDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 
ARRAY IXXT(3) ,IYYT(3) ,IZZT(3) ,IXYT(3),IXZT(3) ,IYZT(3) 

ARRAY YAWANX(3) ,PTCANY(3' 

ARRAY DRCANX ( 3 ) , DRC.ANY ( 3 
ARRAY DRCRAX(3) ,DRCRAY(3 



i),W2(3),W3(3) 

;) ,BRATE1(3) ,BRATE2(3) ,BRATE3(3) 



,ROLANZ( 
,DRCANZ 
,DRCRAZ^ 

ARRAY DRCSX( 3 ) DRCSY( 3 )' , DRCSZ ( 3 ) 
D DATA MATA/ 7 29 « O.ODO/ 



INITIAL 

* INPUT PARAMETER CONSTANTS 

A = 5.0D0 

P = O.ODO 

N = 2.0D0 * PI 

IDGT = 3 

G=0.0D0 

N=27 

M=1 

lA =27 

* INPUT JOINT LOCATIONS IN METERS 

JXO = O.ODO 
JYO = O.ODO 
JZO = O.ODO 
JXl = O.ODO 
JYl = O.ODO 
JZl = l.ODO 
JX2 = O.ODO 
JY2 = l.ODO 
JZ2 = l.ODO 

* INPUT TORQUE CONSTANTS 

TOX = O.ODO 
TOY = O.ODO 
TOZ = O.ODO 
TIY = O.ODO 
TIZ = O.ODO 
T2Y = O.ODO 
T2Z = O.ODO 

* INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 
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FOR EACH LINK ENDS 



L( 

L< 

L( 

L( 

LI 



1,1) 


= 


0.50D0 


1,2) 


= 


0.50D0 


2,1) 


= 


0.50D0 


2,2) 


= 


0.50D0 


3,1) 


= 


0.50D0 


3,2) 




0.50D0 



INPUT MASS AT LINK ENDS IN KILOGRAMS 



MASSl 


:i,i) 


= 


2.5D0 


MASSl 


1,2) 


= 


2.5D0 


MASSl 


2,1) 


= 


2.5D0 


MASSl 


2,2) 


= 


2.5D0 


MASSl 


3,1) 


= 


2.5D0 


MASSl 


[3,2) 


= 


2.5D0 



INPUT OMEGA AND OMEGA DOT, YAW, PITCH, AND ROLL ANGLES 
DO 30 I = 1,3 



W1(I) 

W2(I) 

W3(I) 

WDX(I' 

WDY(I 

WDZ(I 



O.ODO 

O.ODO 

O.ODO 

O.ODO 

O.ODO 

O.ODO 



YAWANX(I) = O.ODO 
PTCANY(I) = O.ODO 
ROLANZ(I) = O.ODO 
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CONTINUE 

YWRXl 

PTRYl 

RLRZl 

YWRX2 

PTRY2 

RLRZ2 

YWRX3 

PTRY3 

RLRZ3 



YAWANXI 

PTCANYi 

ROLANZI 

YAWANXI 

PTCANYI 

ROLANZI 

YAWANXI 

PTCANYI 

ROLANZI 



* 

* 

* 

* 

* 

* 

* 

★ 

* 



DEGRA 

DEGRA 

DEGRA 

DEGRA 

DEGRA 

DEGRA 

DEGRA 

DEGRA 

DEGRA 



INPUT LOCATION OF LINK CENTERS OF GRAVITY 

LCOGX(l) = O.ODO 
XI = LCOGXU) 

LCOGY(l) = O.ODO 
Y1 = LCOGY(l) 

LCOGZ(l) = 0.5D0 
Z1 = LCOGZU) 

LC0GX(2) = O.ODO 
X2 = LC0GX(2) 

LC0GY(2) = 0.5D0 
Y2 = LC0GY(2) 

LC0GZ(2) = l.ODO 
Z2 = LC0GZ(2) 

LC0GX(3) = O.ODO 
X3 = LC0GX(3) 

LC0GY(3) = 1.5D0 
Y3 = LC0GY(3) 

LC0GZ(3) = l.ODO 
Z3 = LC0GZ(3) 

INPUT MASS OF EACH LINK IN KG AND COMPUTE WEIGHTS IN NEWTONS 

MASSl = 5.0D0 
MASS2 = 5.0D0 
MASS3 = 5.0D0 

WGl = MASS1*G 
WG2 = MASS2*G 
WG3 = MASS3’^G 

INPUT ACCELERATION OF JOINT ZERO 
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50 

40 



60 



64 

63 



AOX = O.ODO 
AOY = O.ODO 
AOZ = O.ODO 

INITIALIZE MATRIX A AND B TO ZERO 

DO 40 I = 1,27 
DO 50 J = 1,27 

MATA(I,J) = O.ODO 
CONTINUE 
CONTINUE 

DO 60 I = 1,27 
MATE (I) = O.ODO 
CONTINUE 

INITIALIZE THE TRANSFORMATION MATRICIES AND VELOCITIES 

DO 63 I = 1,3 
DO 64 J = 1,3 

RATEl(I) = O.ODO 
RATE2(I) = O.ODO 
RATE3(I) = O.ODO 
BRATEl(I) = O.ODO 
BRATE2(I) = O.ODO 
BRATE3(I) = O.ODO 

MATIT (I,J) = O.ODO 
MAT2T (I,J) = O.ODO 
MAT3T (I,J) = O.ODO 
MATIR (I,J) = O.ODO 
MAT2R (I,J) = O.ODO 
MAT3R (I,J) = O.ODO 

CONTINUE 

CONTINUE 



DERIVATIVE 
NOSORT 



80 

70 



90 



CALL ERRSET (208,256,-1,1,1) 
LEVELQ = 0 

CALL UERSET(LEVELQ,LEVLDQ) 

INITIALIZE MATRIX A AND B TO ZERO 

DO 70 I = 1,27 
DO 80 J = 1,27 

MATA(I,J) = O.ODO 
CONTINUE 
CONTINUE 

DO 90 I = 1,27 
MATB(I) = O.ODO 
CONTINUE 



:k 

X 

'k 



INPUT JOINT EQUATIONS 
JOINT ZERO EQUATIONS 

AB = AGl + (WDl X RB/Gl) + W1 X (W1 X RB/Gl) 

VECTAO(l) = WDX(l) 

VECTA0(2) = WDY(l) 

VECTA0(3) = WDZ(l) 

RBGl(l) = JXO - LCOGX(l) 

RBG1(2) = JYO - LCOGY(l) 

RBG1(3) = JZO - LCOGZ(l) 

CALL CPROD(VECTAO ,RBG1 ,MIAO ,MJAO ,MKAO) 

VECTAO(l) = Wl(l) 

VECTA0(2) = Wl(2) 

VECTA0(3) = Wl(3) 

CALL CPROD ( VECTAO , RBGl , MIBO , M JBO , MKBO ) 
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VECTBO(l) = MIBO 
VECTB0(2) = MJBO 
VECTBO(3) = MKBO 

CALL CPROD ( VECTAO , VECTBO , MI CO , MJCO , MKCO ) 
JOINT ONE EQUATIONS — 

AA = AGl + (WDl X RA/Gl) + W1 X (Wl X RA/Gl) 

VECTAl(l) = WDX(l) 

VECTA1(2) = WDY(l) 

VECTA1(3) = WDZ(l) 

RAGl(l) = JXl - LCOGX(l) 

RAG1(2) = JYl - LCOGY(l) 

RAG1(3) = JZl - LCOGZ(l) 

CALL CPROD ( VECTAl ,RAG1 ,MIA1 ,MJA1 ,MKA1 ) 

VECTAl(l) = Wl(l) 

VECTAl{2) = Wl(2) 

VECTA1(3) = Wl(3) 

CALL CPROD (VECTA1,RAG1,MIB1,MJB1,MKB1) 

VECTBl(l) = MIBl 
VECTB1(2) = MJBl 
VECTB1(3) = MKBl 

CALL CPROD (VECTA1,VECTB1,MIC1,MJC1,MKC1) 

AB = AG2 + (WD2 X RB/G2) + W2 X (W2 X RB/G2) 

VECTAl(l) = WDX(2) 

VECTA1(2) = WDY(2) 

VECTA1(3) = WDZ(2) 

RBG2(1) = JXl - LCOGX(2) 

RBG2(2)'= JYl - LCOGY(2) 

RBG2(3) = JZl - LCOGZ(2) 

CALL CPROD (VECTA1,RBG2,MIA2,MJA2,MKA2) 

VECTAl(l) = W2(l) 

VECTA1(2) = W2(2) 

VECTA1(3) = W2(3) 

CALL CPROD (VECTA1,RBG2,MIB2,MJB2,MKB2) 

VECTBl(l) = MIB2 
VECTB1(2) = MJB2 
VECTB1(3) = MKB2 

CALL CPROD (VECTA1,VECTB1,MIC2,MJC2,MKC2) 
JOINT TWO EQUATIONS 

AA = AG2 + (WD2 X RA/G2) + W2 X (W2 X RA/G2) 

VECTA2(1) = WDX(2) 

VECTA2(2) = WDY(2) 

VECTA2(3) = WDZ(2) 

RAG2(1) = JX2 - LCOGX(2) 

RAG2(2) = JY2 - LCOGY(2) 

RAG2(3) = JZ2 - LCOGZ(2) 

CALL CPROD (VECTA2,RAG2,MIA3,MJA3,MKA3) 

VECTA2(1) = W2(l) 

VECTA2(2) = W2(2) 

VECTA2(3) = W2(3) 

CALL CPROD (VECTA2,RAG2,MIB3,MJB3,MKB3) 

VECTB2(1) = MIB3 
VECTB2(2) = MJB3 
VECTB2(3) = MKB3 

CALL CPROD ( VECTA2 , VECTB2 , MIC3 , MJC3 , MKC3 ) 
AB = AG3 + (WD3 X RB/G3) + W3 X (W3 X RB/G3) 
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VECTA2(1) = WDX(3) 

VECTA2(2) = WDY(3) 

VECTA2(3) = WDZ(3) 

RBG3(1) = JX2 - LC0GX(3) 

RBG3(2) = JY2 - LC0GY(3) 

RBG3(3) = JZ2 - LC0GZ(3) 

CALL CPROD (VECTA2,RBG3,MIA4,MKA4,MKA4) 

VECTA2(1) = W3(l) 

VECTA2(2) = W3(2) 

VECTA2(3) = W3(3) 

CALL CPROD (VECTA2,RBG3,MIB4,MJB4,MKB4) 

VECTB2(1) = MIB4 
VECTB2(2) = MJB4 
VECTB2(3) = MKB4 

CALL CPROD (VECTA2,VECTB2,MIC4,MJC4,MKC4) 



SUM OF MOMENTS EQUATIONS 
DO 100 I = 1,3 

COMPUTE HX,H DOT X,HY,H DOT Y, HZ,H DOT Z 



RX( 


;i,i) 


= -L( 


;i,i) 


■k 


DCOSi 


(DRCRAX< 


;i)) 


RX( 


1,2) 


= L( 


1,2) 


k 


DCOSi 


DRCRAX( 


l )) 


RY< 


1,1) 


= -L( 


1,1) 


k 


DCOSi 


DRCRAY< 


7) 


RY< 


1,2 


= L( 


1,2 


k 


DCOSI 


DRCRAY( 


I) 


RZ< 




= -L( 


1,1 


k 


DCOSi 


DRCRAZ( 


l) 


RZ< 


1,2) 


= L( 


1,2) 


k 


DCOSi 


(drcrazi 


II)) 



(1,1) 


= MASS (1,1) 


* ((RYI 


;i,i) * RY(I,1); 


> + (RZ(I,1) * RZ(I,1))) 


(1,2) 


= MASS ( I, 2) 


* ((ryi 


[l.2) * RY(I,2): 


) + (RZ(I,2) * RZ(I,2))) 



IF (IXXT(I) .LE. .020) THEN 

IXXT(I) = .020 

ELSE 

IXXT(I) = IXXT(I) 

END IF 



IXY(I,1) = MASS(I,1) * RX(I,1) * RY(I,1 
IXY(I,2) = MASS(I,2) * RX(I,2) * RY(l,2 
IXYT(I) = IXY(I,1) + IXY(I,2) 

IXZ(I,1) = MASS(I,1) * RZ(I,1) * RX(I,1 
IXZ(I,2) = MASS(I,2) * RZ(I,2) * RX(I,2 
IXZT(I) = IXZ(I,1) + IXZ(I,2) 



HDX(l) = WDX(1 
HDX(2) = WDX(2 



* IXX(I,1)- WDZ(I) * IXZ(I,1) 

* IXX(I,2)- WDZ(I) * IXZ(I,2) 



) = MASS( 


1,1) 


* ((RX( 


;i,i) 


* RX( 




> + 


) = MASS( 


,1'?) 


* URX( 


:i,2) 


* RX( 


:i,2): 


) + 



IYY(I,2 
lYYT ( I ) 

IF (lYYT(I) .LE. 
lYYT(I) = .020 
ELSE 

lYYT(I) = lYYT(I) 
END IF 



.020) THEN 



- WDY(I) 


k 


IXY(I,1) 


- wdy(i) 


k 


IXY(I,2) 


(RZ(I,1) 

Uz(I,2) 


k 

k 





IYZ(I,1) = MASS(I,1) * RY(I,1) * RZ(I,1) 

IYZ(I,2) = MASS(I,2) * RY(I,2) * RZ(I,2) 

lYZT(I) = IYZ(I,1) + IYZ{I,2) 

HDY(l) = WDY(I) * IYY(I,1) - WDX(I) * IXY(I,1) - WDZ(I) * IYZ(I,1) 
HDY(2) = WDY(I) * IYY(I,2) - WDX(l) * IXY(I,2) - WDZ(l) * IYZ(l,2) 

IZZ(I,1) = MASS(I,1) * ((RX 

IZZ(I,2) = MASS(I,2) * ((RX 

IZZT(I) = IZZ(I,1) + IZZ(I 

IF (IZZT(I) .LE. .020) THEN 
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IZZT(I) = .020 ■ 
ELSE 

IZZT(I) = IZZT(I) 
END IF 



HDZ(l) = WDZ(I) 
HDZ(2) = WDZ(I) 






* IZZ(I,'2) - WDX 



WDXjlj 



IXZ(I,1) 



IXZ( 



100 



* 

* 

* 



* 

* 



SUMHDX(I) = HDX(l) + HDX(2] 
SUMHDY(I) = HDY(l) + HDY(2 
SUMHDZ(I) = HDZ(l) + HDZ(2] 

CONTINUE 

ENTER CONSTANTS INTO MATRIX A 
'LINK ONE 

SUM OF FORCES IN THE X DIRECTION 

MATA(1,1) = l.ODO- 

MATA(1,4) = MASSl 

MATA(l.lO) = -l.ODO 



MATBUV 
SUM OF FORCES IN 

MATA(2,2) = 

MATA(2,5) = 



MATE (2) = 

SUM OF FORCES IN 



O.ODO 

Y DIRECTION 

l.ODO 
MASSl 

MATA(2;ii) = -l.ODO 
O.ODO 

Z DIRECTION 

MATA(3,3) = l.ODO 

MATA(3,6) = MASSl 

MATA(3,12) = -l.ODO 

SUM OF FORCES LINK ONE EQUAL 
MATE (3) = -WGl 

EQUATIONS AT JOINT ZERO 
IN THE X DIRECTION 

MATA(4,4) = l.ODO 
MATA(4,8) = REG1(3) 

MATA(4,9) = -REG1(2) 

MATE (4) = AOX - MICO' 

IN THE Y DIRECTION 

MATA(5,5) = l.ODO 
MATA(5,7) = -REG1(3) 
MATA(5,9) = REGl(l) 

MATE (5) = AOY - MJCO 

IN THE Z DIRECTION 

MATA(6,6) = l.ODO 
MATA(6,7) = REG1(2) 

MATA(6,8) = -RBGl(l) 

MATE (6) = AOZ - MKCO 



SUM OF MOMENTS EQUATIONS FOR LINK ONE IN THE X, 

2 

1 
1 
3 
2 



MATA(7,2] 




= 


RBGl 


MATA(7,3 




= 


-RBGl 


MATA(7,7 




= 


-IXXT 


MATA(7,8 




= 


IXYT 


MATA(7,9; 






IXZT 


MATA(7,1] 


L) 


= 


-RAGl 


MATA(7,12) 


= 


RAGl 


MATE (7) 




= 


TlX 


MATA (8,1) 


= 


-RBGl 



- WDY(I) * IYZ(I,1 

- WDY(I) * IYZ(I,2 



Y,Z DIRECTIONS 
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X 









X 

X 



■k 



■k 



k 



MATA(8,3) 

MATA(8,7) 

MATA(8,8) 

MATA(8,9) 

MATA(8,10) 

MATA(8,12) 

MATE (8) 

MATA(9,1) 

MATA(9,2) 

MATA(9,7) 

MATA(9,8) 

MATA(9,9) 

MATA (9, 10) 

MATA(9,11) 



= RBGl(l) 

= IXYT(l) 

= -IYYT(l) 

= lYZT(l) 

= RAG1(3) 

= -RAGl(l) 

= TIY - TOY 

= RBG1(2) 

= -RBGl(l) 

= IX2T(1) + 

= lYZT(l) + 
= -IZZT(l) - 
= -RAG1(2) 

= RAGl(l) 



IXZT(2) + IXZT(3) 
IYZT(2) + IYZT(3) 
IZZT(2) - IZZT(3) 



MATB(9) 



TIZ - TOZ 



LINK TWO 



SUM OF FORCES IN X DIRECTION 

MATA(10,10) = l.ODO 
MATA(10,13) = MASS2 
MATA(10,19) = -l.ODO 

MATB(IO) = O.ODO 

SUM OF FORCES IN THE Y DIRECTION 



MATA(11,11) = l.ODO 
MATA(11.14) = MASS2 
MATA(11,20) = -l.ODO 

MATB(ll) = O.ODO 
SUM OF FORCES IN THE Z DIRECTION 



MATA(12,12) = l.ODO 
MATA(12,15) = MASS2 
MATA(12,21) = -l.ODO 

SUM OF FORCES LINK TWO EQUAL 
MATE (12) = -WG2 



EQUATIONS AT JOINT ONE 
IN THE X DIRECTION 

MATA(13,4) = -l.ODO 

MATA(13,8) = -RAG1(3) 

MATA(13,9) = RAG1(2) 

MATA(13,13) = l.ODO 
MATA(13,17) = RBG2(3) 

MATA(13,18) = -RBG2(2) 

MATB(13) = MICl - MIC2 

IN THE Y DIRECTION 

MATA(14,5) = -l.ODO 

MATA(14,7) = RAG1(3) 

MATA(14,9) = -RAGl(l) 

MATA(14,14) = l.ODO 
MATA(14,16) = -RBG2(3) 

MATA(14,18) = RBG2(1) 

MATE (14) = MJCl - MJC2 

IN THE Z DIRECTION 

MATA(15,6) = -l.ODO 

MATA(15,7) = -RAG1(2) 

MATA(15,8) = RAGl(l) 

MATA(15,15) = l.ODO 
MATA(15,16) = RBG2(2) 

MATA(15,17) = -RBG2(1) 

MATE (15) = MKCl - MKC2 

SUM OF MOMENTS EQUATIONS FOR LINK TWO IN THE X,Y,Z DIRECTIONS 
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MATA(16,11) 

MATA(16,12) 

MATA(16,16) 

I1ATA(16,17) 

MATA(16,18) 

MATA(16,20) 

MATA(16,21) 

MATE (16) 

MATA(17,10) 
MATAa7,12) 
MATA(17,16) 
MATA (17, 17) 
MATA (17, 18) 
MATAa7,19) 
MATA(17,21) 



= RBG2(3) 

= -RBG2(2) 

= -IXXT(2) 

= IXYT(2) 

= IXZT(2) 

= -RAG2(3) 

= RAG2(2) 

= (-T1X + T2X) 

= -RBG2(3) 

= RBG2(1) 

= IXyT(2) 

= -IYYT(2) 

= IYZT(2) 

= RAG2(3) 

= -RAG2(1) 



* DCOS(RLRZl) 



MATE (17) 



(- TIY + T2Y) * 



MATA(18,9) = -l.ODO 
MATA(18,18) = l.ODO 
MATE (18) = O.ODO 



Tie 


MATA< 


;i8,10) 


= RBG2 1 


(2) 






"k 


MATA< 


18,11) 


= -RBG2( 


1) 






k 


MATA( 


18,16) 


= IXZTI 


2) 


+ 


IXZT 


k 


MATA( 


18,17) 


= lYZTI 


2) 


+ 


lYZT 


k 


MATA( 


18,18) 


= -IZZTI 


2) 


- 


IZZT 


k 


MATA< 


18,19) 


= -RAG2I 


2) 






k 


MATA< 


;i8,20) 


= RAG2I 


.1) 







(3) 

(3) 

(3) 



MATB(18) = - TIZ + T2Z 



DSIN(RLRZl) 



LINK THREE 



SUM OF FORCES IN THE X DIRECTION 

MATA(19,19) = l.ODO 
MATA(19,22) = MASS3 

MATB(19) = O.ODO 

SUM OF FORCES IN THE Y DIRECTION 



MATA(20,20) = l.ODO 
MATA (20, 23) = MASS3 

MATB(20) = O.ODO 

SUM OF FORCES IN THE Z DIRECTION 



MATA(21,21) = l.ODO 
MATA(21,24) = MASS3 

MATE (21) = -WG3 

EQUATIONS AT JOINT TWO 

IN THE X DIRECTION 



* 



MATA( 


122,13; 


) = 


-l.ODO 


MATA< 


22,17 


1 — 


-RAG2( 


MATA( 


22,18 


) — 


RAG2( 


MATAI 


22,22 


1 = 


l.ODO 


MATAI 


22,26 


) = 


RBG3( 


MATA( 


22,27 


^ = 


-RBG3( 


MATE (22) 




MIC3 


THE Y 


DIRECTION 


MATA( 


[23,14; 


t — 


-l.ODO 


MATA( 


23,16 


1 = 


RAG2( 


MATA( 


23,18 


) = 


-RAG2( 


MATAI 


23,23 


1 = 


l.ODO 


MATAI 


23,25 


) = 


-RBG3( 


MATA( 


[23,27 


) = 


RBG3( 


MATE (23) 


= 


MJC3 






MIC4 



MJC4 
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IN THE Z DIRECTION 



* 

X 

•k 

k 

k 

k 

*18 

*118 

k 

k 

k 

k 

*81 
181 



MATA( 


[24,15) 


=5 


-l.ODO 


MATA< 


24,16) 


= 


-RAG2(2) 


MATA< 


24,17) 


= 


RAG2(1) 


MATA( 


24,24) 


= 


l.ODO 


MATA< 


24,25) 


= 


REG3(2) 


MATA( 


[24,26) 




-REG3(1) 



MATE (24) = MKC3 - MKC4 

SUM OF MOMENTS EQUATIONS FOR LINK THREE IN THE X,Y,2 DIRECTIONS 



MATAI 


[25, 2o; 


) == 


REG3I 


[3) 


MATAI 


25,21 


) = 


-REG3I 


2 


MATAI 


25,25 


) = 


-IXXTI 


3 


MATAI 


25,26 


1 = 


IXYTI 


3 


MATAI 


25,27 




IXZTI 


3 


MATE (25) 


= 


-T2X * E 


MATAI 


[26,19; 


) = 


-REG3I 


[3) 


MATAI 


26,21 


) = 


REG3I 


1 


MATAI 


26,25 


1 = 


IXYTI 


3 


MTAI 


26,26 


) = 


-lYYTl 


3 


MATAI 


[26,27 


) = 


lYZTI 


[3) 



DCOS(RLRZl) 



MATE (26) 

MATA (27, 9) 
MATA(27,27) 
MATE (27) 



= -T2Y * DSIN(RLRZl) 

= -l.ODO 
= l.ODO 
= O.ODO 



k 


MATA( 


[27,19) 


= REG3(2) 


k 


MATAI 


27,20) 


= -REG3(l) 


k 


MATAI 


27,25) 


= IXZTI3) 


k 


MATAI 


27,26) 


= IYZT(3) 


k 


MATAI 


27,27) 


= -IZZT(3 



MATE(27) = - T2Z 
GO TO 1112 

INITIALIZE MATRIX ACCORDING TO CONSTRAINTS 
CONSTRAINTS GROUP 1 WHEN ONLY LINK THREE IS IN MOTION 

DO 118 I = 1,18 
DO 18 J = 1,27 
MATA(I,J) = 0.0 
MATA(I,I) = 1.0 
MAT8(I) =0.0 
CONTINUE 
CONTINUE 

DO 181 I = 19,27 
DO 81 J = 1,18 
MATAU,J) = 0.0 
CONTINUE 
CONTINUE 
GO TO 1111 

CONSTRAINTS GROUP 2 WHEN LINK TWO AND THREE ARE 'iN MOTION 



k 

k 


DO 19 I = 
DO 191 


= 1,9 
J = 1 


,27 


k 


MATAI 


;i,j) 


= O.ODO 


k 


MATAI 


1,1) 


= 1.0 DO 


k 

k 


MATEl 




= O.ODO 


k 


mATAl 


[17, J) 


= O.ODO 


k 


MATAI 


18. J 


= O.ODO 


k 


MATE 1 


17) 


= O.ODO 


k 

X 


MATEl 


18 


= O.DO 


k 


MATA(J,17) 


= O.ODO 
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* MATA(J,18) = O.ODO 

* mATA(17,l7) = l.ODO 

* MATA(18,18) = l.ODO 

* 

*191 CONTINUE 

*19 CONTINUE 

* 

* DO 91 I = 10,27 

* DO 92 J = 1,9 

* mataU,J) = 0.0 

*92 CONTINUE 

*91 CONTINUE 

** GO TO 1111 



CONSTRAINTS GROUP 3 WHEN THREE OF THE LINKS ARE IN MOTION 



* 

* 

* 

"k 

k 

k 



k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 



DO 78 J = 1,27 




MATA< 


7,J) 


= 


O.ODO 


MATA< 


8,jj 


= 


O.ODO 


MATA( 


J,7 


= 


O.ODO 


MATA< 


J<8) 


= 


O.ODO 


MATB< 


7 


= 


O.ODO 


MATBI 


[B) 


= 


O.ODO 


MATA< 


;i7,J) 


= 


O.ODO 


MATA< 


18, J) 


= 


O.ODO 


MATAI 






O.ODO 


MATA< 


J,18) 




O.ODO 


MATB< 


17) 


= 


O.ODO 


MATB( 


!18) 




O.ODO 


MATA< 


;26,J) 


= 


O.ODO 


MATA< 


27, J) 


= 


O.ODO 


MATA< 


J,26) 




O.ODO 


MATA< 


J,27) 


= 


O.ODO 


MATB< 


26) 


= 


O.ODO 


MATB< 


[27) 


= 


O.ODO 


MATA< 


[7,7) 




= l.ODO 


MATA< 


8,8) 




= l.ODO 


MATAI 


17,17 




= l.ODO 


MATA( 


18,18 




= l.ODO 


MATA< 


26,26 




= l.ODO 


MATA( 


27,27 




= l.ODO 



*78 CONTINUE 



* CONSTRAINT GROUP 4 THE FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIR. 

1112 DO 48 I = 4,8 

DO 481 J = 1,27 

MATA(I,J) = O.ODO 
MATA(I,I) = 1.0 DO 
MATB(I) = O.ODO 
481 CONTINUE 

48 CONTINUE 

DO 84 I = 9,27 
DO 841 J = 4,8 
MATA(I,J) = 0.0 
841 CONTINUE 

84 CONTINUE 

* call equation solver program from IMSL 

1111 CALL LEQT2F(MATA,M,N,IA,MATB,IDGT,WKAREA,IER) 

* 'IF (lER .NE. 0) CALL ENDJOB 



FIND LCOGX,LCOGY,LCOGZ, THETA VALUES ,WX,WY,WZ 
LINK ONE 

AXl = MATB(4) 
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A- ^ ^ ^ 



606 

605 



* 



VELXl = INTGRLfO. ,AX1) 

LCOGXl = INTGRL (XI, VELXl) 

LCOGX(l) = LCOGXl 

AYl = MATB(5) 

VELYl = INTGRL (0. , AYl) 

LCOGYl = INTGRL (yi, VELYl) 

LCOGY(l) = LCOGYl 

AZl = NATB(6) 

VELZl = INTGRL(0. ,AZ1) 

LCOGZl = INTGRL (Zl, VELZl) 

LCOGZ(l) = LCOGZl 

WDIX = MATB(7) 

WIX = INTGRL (0. , WDIX) 

WDX(l) = WDIX 

Wl(l) = WIX 

WDIY = MATB(8) 

WIY = INTGRL (0. , WDIY) 

WDY(l) = WDIY 

Wl(2) = WIY 

WDIZ = MATB(9) 

WIZ = INTGRL ( 0. , WDIZ) 

WDZ(l) = WDIZ 

Wl(3) = WIZ 



TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
SYSTEM FOR LINK ONE 



MAT1R(1,1) = DCOS(RLRZl) * DCOS(PTRYl) 

MAT1R(2,1) = DCOS(RLRZl) * DSIN(PTRYl) * DSIN(YWRXl) 
DSIN(RLRZl) * DCOS(YWRXl) 

MAT1R(3,1) = DCOS(RLRZl) * DSIN(PTRYl) * DCOS(YWRXl) +... 
DSIN(RLRZl) * DSIN(YWRXl) 

MAT1R(1,2) = DSIN(RLRZ1)*DC0S(PTRY1) 

MAT1R(2,2) = DSIN(RLRZl) * DSIN(PTRYl) * DSIN(YWRXl) +. . . 
DCOS(RLRZl) * DCOS(YWRXl) 

MAT1R(3,2) = DSIN(RLRZl) * DSIN(PTRYl) * DCOS(YWRXl) 
DCOS(RLRZl) * DSIN(YWRXl) 

MAT1R(1,3) = -DSIN(PTRYl) 

MAT1R(2,3) = DCOS(PTRYl) * DSIN(YWRXl) 

MAT1R(3,3) = DCOS(PTRYl) * DCOS(YWRXl) 

GET THE VELOCITIES OF LINK ONE IN BODY FIXED COORDINATE SYSTEM 



DO 605 J = 1,3 
SUMl = O.ODO 
DO 606 K = 1,3 

SUMl = SUMl + MAT1R(J,K) * W1(K) 
CONTINUE 

BRATEl(J) = SUMl 
CONTINUE 



TRANSFORMATION MATRIX FROM BODY FIXED TO NON-ORTHOGONAL EULER 
COORDINATE SYSTEM FOR LINK ONE 

MAT1T(1,1) = O.ODO 
MAT1T(2,1) = l.ODO 
MAT1T(3,1) = O.ODO 

MAT1T(1,2) = DCOS(YWRXl) 

MAT1T(2,2) = DTAN(PTRYI) * DSIN(YWRXl) 

MAT1T(3,2) = 1.0DO/DCOS(PTRY1) ^ DSIN(YWRXl) 

MAT1T(1,3) = -DSIN(YWRXl) 

MAT1T(2,3) = DTAN(PTRYI) * DCOS(YWRXl) 

MAT1T(3,3) = l.DO/DCOS(PTRYl) * DCOS(YWRXl) 

GET THE VELOCITIES OF LINK ONE IN THE EULER COORDINATE SYSTEM 
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706 

705 

* 



* 

9 



DO 705 J = 1,3 
SUMl = O.ODO 
DO 706 K = 1,3 

SUMl = SUMl + MAT1T(J,K) * BRATEl(K) 

CONTINUE 
RATEl(J) = SUMl 
CONTINUE ■ 

RATEIX = RATEl(l) 

RATEIY = RATE1(2) 

RATEIZ = RATE1(3) 

INTEGRATION OF THE VELOCITIES OF LINK ONE IN EULER COOR. SYSTEM 

YWRXl = INTGRL(0. ,RATE1X) 

PTRYl = INTGRL(0. , RATEIY) 

RLRZl = INTGRL ( -PI/2. , RATEIZ) 

CONVERT THE ANGLES TO DEGREES 

YAWANX(l) = YWRXl * RADEG 

PTCANY(l) = PTRYl * RADEG 

ROLANZ(l) = RLRZl * RADEG 

GET THE DIRECTION COSINES FOR THE LINK ONE 

DRCSY(l) = DCOS(RLRZl) * DSIN(PTRYl) * DCOS(YWRXl) +... 

DSIN (RLRZl) * DSIN( YWRXl) 

DRCSX(l) = DSIN(RLRZl) * DSIN(PTRYl) * DCOS (YWRXl) 
DCos(RLRZi) * dsin(yv;rxi) 

DRCSZ(l) = DCOS (PTRYl) * DCOS (YWRXl) 

DRCRAX(l) = DACOS(DRCSX(l)) 

DRCRAY(l) = DACOS(DRCSY(l)) 

DRCRAZ(l) = DACOS(DRCSZ(l)) 

DRCANX(l) = DAC0S(DRCSX(1)) * RADEG 

DRCANY(l) = DACOS(DRCSY(l)) * RADEG 

DRCANZ(l) = DACOS(DRCSZ(l)) * RADEG 



LINK TWO 



AX2 

VELX2 

LCOGX2 

LCOGX(2) 

AY2 

VELY2 

LCOGY2 

LC0GY(2) 

AZ2 

VELZ2 

LC0GZ2 

LCOGZ(2) 

WD2X 

W2X 

WDX(2) 

W2(l) 



MATE (13) 

= INTGRL (0. 
= INTGRL (X2 
= LC0GX2 

= MATE (14) 

= INTGRL(0. 
= INTGRL (Y2 
= LCOGY2 

= MATE (15) 

= INTGRL (0. 
= INTGRL (Z2 
= LCOGZ2 



,AX2) 

,VELX2) 



.AY2) ^ 
,VELY2) 



,AZ2) ^ 
,VELZ2) 



MATE (16) 

INTGRL ( 0. ,WD2X) 

WD2X 

W2X 



WD2Y 

W2Y 

WDY(2) 

W2(2) 



MATE (17) 

INTGRL ( 0. ,WD2Y) 

WD2Y 

W2Y 



WD2Z 

W2Z 

WDZ(2) 

W2(3) 



MATE(18) 

INTGRL ( 0. ,WD2Z) 

WD2Z 

W2Z 



TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
SYSTEM FOR LINK TWO 



MAT2R(1,1) = DCOS(RLRZ2) * DCOS(PTRY2) 
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■k 



608 

607 



* 

k 



k 



708 

707 



k 



k 



k 



MAT2R(2,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) 
DSIN(RLRZ2) * DCOS(YWRX2) 

MAT2R(3,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) +... 
DSIN(RLRZ2) * DSIN(YWRX2) 

MAT2R(1,2) = DSIN(RLRZ2) * DCOS(PTRY2) 

MAT2R(2,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) +... 
DCOS(RLRZ2) * DCOS(YWRX2) 

MAT2R(3,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) 
DCOS(RLRZ2) * DSIN(YWRX2) 

MAT2R(1,3) = -DSIN(PTRY2) 

MAT2R(2,3) = DCOS(PTRY2) * DSIN(YWRX2) 

MAT2R(3,3) = DCOS(PTRY2) * DCOS(YWRX2) 

GET THE VELOCITIES OF LINK TWO IN BODY FIXED COORDINATE SYSTEM 

DO 607 J = 1,3 
SUMl = O.ODO 
DO 608 K = 1,3 

SUMl = SUMl + MAT2R(J,K) * W2(K) 

CONTINUE 

BRATE2(J) = SUMl 
CONTINUE 



TRANSFORMATION MATRIX FROM BODY FIXED TO NON-ORTHOGONAL EULER 
COORDINATE SYSTEM FOR LINK TWO 



MAT2T(1,1) = O.ODO 
MAT2T(2,1) = l.ODO 
MAT2T(3,1) = O.ODO 

MAT2T(1,2) = DCOS(YWRX2) 

MAT2T(2,2) = DTAN(PTRY2) * DSIN(YWRX2) 
MAT2T(3,2) = 1 . ODO/DCOS (PTRY2 ) * DSIN(YWRX2) 



MAT2T(1,3) = -DSIN(YWRX2) 

MAT2T(2,3) = DTAN(PTRY2) * DCOS(YWRX2) 
MAT2T(3,3) = 1 .ODO/DCOS (PTRY2) * DCOS(YWRX2) 



GET THE VELOCITIES OF LINK TWO IN THE EULER COORDINATE SYSTEM 



DO 707 J = 1,3 
SUMl = O.ODO 
DO 708 K = 1,3 

SUMl = SUMl + MAT2T(J,K) * BRATE2(K) 
CONTINUE 
RATE2(J) = SUMl 
CONTINUE 



RATE2X = RATE2(1) 

RATE2Y = RATE2(2) 

RATE2Z = RATE2(3) 

INTEGRATION OF THE VELOCITIES OF LINK TWO IN EULER COOR. SYSTEM 

YWRX2 = INTGRL(0. ,RATE2X) 

PTRY2 = INTGRL(0. ,RATE2Y) 

RLRZ2 = INTGRL(-PI/2. ,RATE2Z) 

CONVERT- THE ANGLES TO DEGREES 

YAWANX(2) = YWRX2 * RADEG 

PTCAirY(2) = PTRY2 * RADEG 

ROLANZ(2) = RLRZ2 * RADEG 

GET THE DIRECTION COSINES FOR THE LINK TWO 

DRCSY(2) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) -... 
DSIN(RLRZ2) * DCOS(YWRX2) 

DRCSX(2) = DSIN(RLRZ2) * DSIN(PTRY2)*DSIN(YWRX2) +... 
DCOS(RLRZ2) * DCOS(YWRX2) 

DRCSZ(2) = DCOS(PTRY2) * DSIN(YWRX2) 

DRCRAX(2) = DACOS(DRCSX(2)) 
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DRCRAY(2) = DAC0S(DRCSY(2)) 

DRCRAZ(2) = DAC0S(DRCSZ(2)) 

DRCANX(2) = DAC0S(DRCSX(2)) * RADEG 
DRCANY(2) = DAC0S(DRCSY(2)) * RADEG 
DRCANZ(2) = DAC0S(DRCSZ(2)) * RADEG 

JXl = (L(l,l) + L(l,2)) * DC0S(DRCRAX(1)) 
JYl = (L(l,l) + L(l,2)) * DC0S(DRCRAY(1)) 
JZl = (L(l/1) + L(l,2)) * DC0S(DRCRAZ(1)) 



* 

6 






•k 



610 

609 



LINK THREE 

AX3 
VELX3 
LCOGX3 
LCOGX(3) 

AY3 
VELY3 
LCOGY3 
LCOGY(3) 

AZ3 
VELZ3 
LCOGZ3 
LCOGZ(3) 

WD3X 
W3X 
WDX(3) 

W3(l) 

WD3Y 
W3Y 
WDY(3) 

W3(2) 

WD3Z 
W3Z 
WDZ(3) 

W3(3) 

TRANSFORMATION MATRIX 
SYSTEM FOR LINK THREE 



MATE (22) 

= INTGRL(0. ,AX3) 

= INTGRL(X3,VELX3) 
= LCOGX3 

= MATE (23) 

= INTGRL(0. ,AY3) 

= INTGRL(Y3,VELY3) 
= LCOGY3 

= MATE (24) 

= INTGRL(0. ,AZ3) 

= INTGRL(Z3,VELZ3) 
= LCOGZ3 

= MATE(25) 

= INTGRL(0. ,WD3X) 

= WD3X 
= W3X 

= MATE(26) 

= INTGRL(0. ,WD3Y) 

= WD3Y 
= W3Y 



MATE (27) 
INTGRL(0. 
WD3Z 
W3Z 



,WD3Z) 



FROM EARTH FIXED TO EODY FIXED COORDINATE 



MAT3R(1,1) = DCOS(RLRZ3) * DCOS(PTRY3) 

MAT3R(2,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) 
DSIN(RLRZ3) * DC0S(YWRX3) 

MAT3R(3,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) +... 
DSIN(RLRZ3) * DSIN(YWRX3) 

MAT3R(1,2) = DSIN(RLRZ3) * DCOS(PTRY3) 

MAT3R(2,2) = DSIN(RLRZ3) DSIN(PTRY3) * DSIN(YWRX3) +... 
DCOS(RLRZ3) * DCOS(YWRX3) 

MAT3R(3,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) 
DCOS(RLRZ3) * DSIN(YWRX3) 

MAT3R(1,3) = -DSIN(PTRY3) 

MAT3R(2,3) = DCOS(PTRY3) * DSIN(YWRX3) 

MAT3R(3,3) = DCOS(PTRY3) ’^DCOS(YWRX3) 

GET THE VELOCITIES OF LINK THREE IN EODY FIXED COORDINATE SYSTEM 



DO 609 J = 1,3 
SUMl = O.ODO 
DO 610 K = 1,3 

SUMl = SUMl + MAT3R(J,K) * W3(K) 
CONTINUE 

ERATE3(J) = SUMl 
CONTINUE 



TRANSFORMATION MATRIX FROM EODY FIXED TO NON-ORTHOGONAL EULER 
COORDINATE SYSTEM FOR LINK THREE 
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MAT3T(1,1) = O.ODO 
MAT3T(2,1) = l.ODO 
MAT3T(3,1) = O.ODO 

MAT3T(1,2) = DCOS(YWRX3) 

MAT3T(2,2) = DTAN(PTRY3) * DSIN(YWRX3) 

MAT3T(3,2) = 1 .ODO/DCOS(PTRY3) * DSIN(YWRX3) 

MAT3T(1,3) = -DSIN(YWRX3) 

MAT3T(2,3) = DTAN(PTRY3) * DC0S(YWRX3) 

MAT3T(3,3) = 1 . ODO/DCOS (PTRY3 ) * DCOS(YWRX3) 

GET THE VELOCITIES OF LINK THREE IN THE EULER COORDINATE SYSTEM 

DO 709 J = 1,3 
sum = O.ODO 
DO 710 K = 1,3 

SUMl = SUMl + MAT3T(J,K) * BRATE3(K) 

710 CONTINUE 

RATE3(J) = SUMl 
709 CONTINUE 

RATE3X = RATES (1) 

RATES Y = RATE3(2) 

RATE3Z = RATES (3) 

INTEGRATION OF THE VELOCITIES OF LINK THREE IN EULER COOR. SYSTEM 

YWRX3 = INTGRL(0. ,RATE3X) 

PTRY3 = INTGRL(0. ,RATE3Y) 

RLRZ3 = INTGRL(-PI/2. ,RATE3Z) 

CONVERT THE. ANGLES TO DEGREES 

YAWANX(3) = YWRX3 * RADEG 

PTCANY(3) = PTRY3 * RADEG 

ROLANZ(3) = RLRZ3 * RADEG 

GET THE DIRECTION COSINES FOR THE LINK THREE 

DRCSY(3) = DCOS(RLRZS) * DSIN(PTRY3) * DSIN(YWRX3) -... 
DSIN(RLRZ3) * DCOS(YWRX3) 

DRCSX(3) = DSIN(RLRZ3) * DSIN(PTRYS) * DSIN(YWRX3) +... 
DCOS(RLRZ3) * DCOS(YWRXS) 

DRCSZ(3) = DCOS(PTRYS) * DSIN(YWRXS) 

DRCRAX(3) = DACOS(DRCSX(3)) 

DRCRAY(3) = DACOS(DRCSY(3 ) 

DRCRAZ(3) = DACOS(DRCSZ(3)) 

DRCANX(3) = DACOS(DRCSX(3)) * RADEG 
DRCANY(3) = DACOS(DRCSY(3)) * RADEG 
DRCANZ(3) = DACOS(DRCSZ(3)) * RADEG 

JX2 = JXl + (L(2,l) + L(2,2)) * DCOS(DRCRAX(2) ) 

JY2 = JYl + (L(2,l) + L(2,2)) * DCOS(DRCRAY(2) ) 

JZ2 = JZl + (L(2,l) + L(2,2)) * DCOS(DRCRAZ(2) ) 

TIPX = JX2 + (L(3,l) + L(3,2)) * DCOS(DRCRAX(3) ) 

TIPY = JY2 + (L(3,l) + L(3,2)) * DCOS(DRCRAY(3) ) 

TIPZ = JZ2 + (L(3,l) + L(3,2)) * DCOS(DRCRAZ(3) > 



DYNAMIC 

NOSORT 

* INPUT TORQUE AT JOINTS 

TOZ = A * SIN (W * TIME + P) 

TlX =-10 * SIN (W * TIME + P) 

T2X = A * SIN (W * TIME + P) 



END 

STOP 

FORTRAN 
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* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 

SUBROUTINE CPROD ( VECTA , VECTB , MI , MJ , MK) 

IMPLICIT REAL*8 (A-Z) 

DIMENSION VECTA(3) ,VECTB(3) 

MI = VECTA(2) * VECTB(3) - VECTA(3) * VECTB(2) 

MJ = VECTA(3) * VECTB(l) - VECTA(l) * VECTB(3) 

MK = VECTA(l)' * VECTB(2) - VECTA(2) * VECTB(l) 

RETURN 
END 
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APPENDIX C 



THREE DIMENSIONAL SIMULATION PROGRAM 
INVESTIGATION OF SINGULAR CONFIGURATION 



TERMINAL 
METHOD ADAMS 

PRINT .05, ERROR, ANG12Z,ANG23Z 

CONTROL FINTIM =4.0, DELMAX =.l, DELPRT = .05 

SAVE .05, ERROR, ANG12X,ANG12Y,ANG12Z,THETAB,THETAD,ANG23X,ANG23Y, . . . 
ANG23Z,IYYT(2) ,IXXT(2) ,IZZT(2) 



GRAPH (DE=TEK6 18 
GRAPH(DE=TEK618 
GRAPH (DE=TEK6 18 
GRAPH (DE=TEK6 18 
GRAPH (DE=TEK6 18 
GRAPH (DE=TEK6 18 
GRAPH (DE=TEK6 18 
GRAPH (DE=TEK6 18 



D 

D 

D 

D 

D 



TIME,ANG12X 
TIME-,ANG12Y 
TIME,ANG12Z 
TIME,ANG23X 
TIME,ANG23Y 
TIME,ANG23Z 
TIME,THETAB 

TIME,IYYT(2) ,IXXT(2) ,IZZT(2) 

DIMENSION MATA(27, 27) , MASS (3, 2) ,L(3 , 2 ) ,RX(3 , 2) ,RY(3,2), 
DIMENSION IXX(3,2) ,IXZ(3,2) ,IXY(3,2) , lYY (3 , 2 ) , IYZ(3 ,2) , 



DIMENSION MAT1R(3,3), 

DIMENSION MAT1T(3,3) , 

INTEGER IER,I, J,M,K,P,N,IA,IDGT,A 



MAT2R(3,3) ,MAT3R(3,3 
MAT2T(3,3) ,MAT3T(3,3 



RZ(3,2) 

IZZ(3,2) 



EXCLUDE IA,IDGT,IER,I, J,M,K,P,N,A 
ARRAY MATB(27) ,LCOGX(3) ,LCOGyU) ,LCOGZ(3) 

ARRAY VECTA0(3) , VECTBO (3 ) , VECTAl (3 ) ,VECTB1(3) ,VECTA2(3) ,VECTB2(3) 
ARRAY WDX(3) ,WDY(3) ,WDZ(3) ,W1 (3) ,W2(3) ,W3(3) ,MATC (27 ) ,DQU7 ) 

ARRAY RATE1(3) ,RATE2(3) ,RATE3(3) ,BRATE1 (3) ,BRATE2(3) ,BRATE3(3) 

ARRAY RBG1(3) ,RAG1 (3 ) , RBG2 (3) ,RAG2(3) ,RBG3(3) 

ARRAY SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 
ARRAY IXXT(3) ,IYYT(3) ,IZZT(3) ,IXYT(3) ,IXZT(3) ,IYZT(3) 



ARRAY YAWANX 
ARRAY DRCANX 
ARRAY DRCRAX 
ARRAY 
D 



;3) ,PTCANY(3) ,R0LANZ(3) 
3) ,DRCANY(3) ,DRCANZ(3) 
:3) ,DRCRAY(3) ,DRCRAZ(3) 
DRCSX(3) ,DRCSY(3) ,DRCSZ(3) 
DATA MATA/729 O.ODO/ 



INITIAL 

* INPUT PARAMETER CONSTANTS 

A = 3.0D0 

P = O.ODO 

W = PI / 2.0D0 

IDGT = 3 

G=0.0D0 

N=27 

M=1 

lA =27 

* INPUT JOINT LOCATIONS IN METERS 

JXO = O.ODO 
JYO = O.ODO 
JZO = O.ODO 
JXl = O.ODO 
JYl = O.ODO 
JZl = l.ODO 

* USE THE NEXT SET OF JOINT TWO COORDINATES FOR CASE A 

JX2 = O.ODO 
JY2 = l.ODO 
JZ2 = l.ODO 

* USE THE NEXT SET OF JOINT TWO COORDINATES FOR CASE B 
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■k 

* 

* 

* 

* 

* 

* 

* 

* 

* 



* 



* 



30 



* 



* 

* 

* 

* 

* 

* 

* 



JX2 = l.ODO 
JY2 = O.ODO 
JZ2 = l.ODO 

USE THE NEXT SET OF JOINT TWO COORDINATES FOR CASE C 



JX2 = O.ODO 
JY2 = O.ODO 
JZ2 = 2.0D0 



INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS FOR 
EACH LINK ENDS 



1,1) 


= 


0.50D0 


1,2) 


= 


0.50D0 


2,1) 


= 


0.50D0 


2,2) 




0.50D0 


3,1) 


= 


0.50D0 


3,2 




0.50D0 



INPUT MASS AT LINK ENDS IN KILOGRAMS 



MASS! 


1,1) 


= 


2.5D0 


MASSI 


1,2) 




2.5D0 


MASS! 


2,1) 


= 


2.5D0 


MASSI 


2,2) 


= 


2.5D0 


MASSI 


3,1) 


= 


2.5D0 


MASSI 


3,2) 


= 


2.5D0 



INPUT OMEGA AND OMEGA DOT 



DO 30 I = 1,3 

W1(I) = O.ODO 

W2(I) = O.ODO 

W3(l) = O.ODO 

WDX(I) = O.ODO 

WDY(l) = O.ODO 

WDZ(I) = O.ODO 

CONTINUE 



INPUT LOCATION OF LINK CENTERS OF GRAVITY 
LINK ONE 

LCOGX(l) = O.ODO 
XI = LCOGXU) 

LCOGY(l) = O.ODO 
Y1 = LCOGY(l) 

LCOGZ(l) = 0.5D0 
Z1 = LCOGZ(l) 

NEXT SET FOR LINK TWO AND THREE TO USE FOR CASE A 

LC0GX(2) = O.ODO 
X2 = LC0GX(2) 

LC0GY(2) = 0.5D0 
Y2 = LC0GY(2) 

LC0GZ(2) = l.ODO 
Z2 = LCOGZU) 

LC0GX(3) = O.ODO 
X3 = LC0GX(3) 

LC0GY(3) = 1.5D0 
Y3 = LC0GY(3) 

LC0GZ(3) = l.ODO 
Z3 = LC0GZ(3) 

NEXT SET FOR LINK TWO AND THREE TO USE FOR CASE B 

LC0GX(2) = 0.5D0 
X2 = LC0GX(2) 

LC0GY(2) = O.ODO 
Y2 = LC0GY(2) 

LC0GZ(2) = l.ODO 
Z2 = LC0GZ(2) 

LC0GX(3) = 1.5D0 
X3 = LC0GX(3) 
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'k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 

k 



k 



k 



so 

40 



60 



* 



64 

63 



LC0GY(3) = O.ODO 
Y3 = LC0GY(3) 

LC0GZ(3) = l.ODO 
Z3 = LC0GZ(3) 

NEXT SET FOR LINK' TWO AND THREE TO USE FOR CASE C 

LC0GX(2) = O.ODO 
X2 = LC0GX(2) 

LC0GY(2) = O.ODO 
Y2 = LC0GY(2) 

LC0GZ(2) = 1.5D0 
Z2 = LC0GZ(2) 

LC0GX(3) = O.ODO 
X3 = LC0GX(3) 

LC0GY(3) = O.ODO 
Y3 = LC0GY(3) 

LC0GZ(3) = 2.5D0 
Z3 = LC0GZ(3) 

INPUT MASS OF EACH LINK IN KG AND COMPUTE WEIGHTS IN NEWTONS 

MASSl = 5.0D0 
MASS2 = 5.0D0 
MASS3 = 5.0D0 

WGl = MASSING 
WG2 = MASS2*G 
WG3 = MASS3*G 

INPUT ACCELERATION OF JOINT ZERO 

AOX = O.ODO 
AOY = O.ODO 
AOZ = O.ODO 

INITIALIZE MATRIX A AND B TO ZERO 

DO 40 I = 1,27 
DO 50 J = 1,27 

MATA(I,J) = O.ODO 
DQ(I) = O.ODO 
MATC(I) = O.ODO 
CONTINUE 
CONTINUE 

DO 60 I = 1,27 
MATB(I) = O.ODO 
CONTINUE 

INITIALIZE THE INITIAL VELOCITIES AND TRANSFORMATION MATRICIES 

DO 63 I = 1,3 
DO 64 J = 1,3 

RATEl(I) = O.ODO 
RATE2(I) = O.ODO 
RATE3(I) = O.ODO 
BRATEl(I) = O.ODO 
BRATE2(l) = O.ODO 
BRATE3(I) = O.ODO 

MATIT (I,J) = O.ODO 
MAT2T (I,J) = O.ODO 
MAT3T (I,J) = O.ODO 
MATIR (I,J) = O.ODO 
MAT2R (I,J) = O.ODO 
MAT3R (I,J) = O.ODO 

CONTINUE 

CONTINUE 



DERIVATIVE 
NO SORT 

CALL ERRSET (208,256,-1,1,1) 
LEVELQ = 0 

CALL UERSET( LEVELQ, LEVLDQ) 
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INITIALIZE MATRIX A AND B TO ZERO 

DO 70 I = 1,27 
DO 80 J = 1,27 
■ MATA(I,J) = O.ODO 
CONTINUE 
CONTINUE 

DO 90 I = 1,27 
MATB(I) = O.ODO 
DQ(I) = O.ODO 
CONTINUE 



INPUT JOINT EQUATIONS 
JOINT ZERO EQUATIONS 

AB = AGl + (WDl X RB/Gl) + W1 X (W1 X RB/Gl) 

VECTAO(l) = WDX(l) 

VECTA0(2) = WDY(l) 

VECTA0(3) = WDZ(l) 

RBGl(l) = JXO - LCOGX(l) 

RBG1(2) = JYO - LCOGY(l) 

RBG1(3) = JZO - LCOGZ(l) 

CALL CPROD ( VECT AO , RBGl , MI AO , M JAO , MKAO ) 

VECTAO(l) = Wl(l) 

VECTA0(2) = Wl(2) 

VECTA0(3) = Wl(3) 

CALL CPROD ( VE CTAO , RBGl , MIBO , M JBO , MKBO ) 

VECTBO(l) = MIBO 
VECTB0(2) = MJBO 
VECTB0(3) = MKBO 

CALL CPROD (VECTAO , VECTBO ,MICO ,MJCO ,MKCO ) 
JOINT ONE EQUATIONS--- 

AA = AGl + (WDl X RA/Gl) + W1 X (W1 X RA/Gl) 

VECTAl(l) = WDX(l) . 

VECTA1(2) = WDY(l) 

VECTA1(3) = WDZ(l) 

RAGl(l) = JXl - LCOGX(l) 

RAG1(2) = JYl - LCOGY(l) 

RAG1(3) = JZl - LCOGZ(l) 

CALL CPROD (VECTAl ,RAG1 ,MIA1 ,MJA1 ,MKA1) 

VECTAl(l) = Wl(l) 

VECTA1(2) = Wl(2) 

VECTAl (3) = Wl(3) 

CALL CPROD (VECTAl, RAG1,MIB1,MJB1,MKB1) 

VECTBl(l) = MIBl 
VECTB1(2) = MJBl 
VECTB1(3) = MKBl 

CALL CPROD (VECTAl, VECTB1,MIC1,MJC1,MKC1) 
AB = AG2 + (WD2 X RB/G2) + W2 X (W2 X RB/G2) 

VECTAl(l) = WDX(2) 

VECTAl (2) = WDY(2) 

VECTAl (3) = WDZ(2) 

RBG2(1) = JXl - LCOGX(2) 

RBG2(2) = JYl - LCOGY(2) 

RBG2(3) = JZl - LCOGZ(2) 

CALL CPROD (VECTAl, RBG2,MIA2,MJA2,MKA2) 

VECTAl (1) = W2(l) 

VECTA1(2) = W2(2) 



VECTA1(3) = W2(3) 

CALL CPROD (VECTA1,RBG2,MIB2,MJB2,MKB2) 

VECTBl(l) = MIB2 
VECTB1(2) = MJB2 
VECTB1(3) = MKB2 

CALL CPROD (VECTA1,VECTB1,MIC2,MJC2,MKC2) 
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JOINT TWO EQUATIONS 

AA = AG2 + (WD2 X RA/G2) + W2 X (W2 X RA/G2) 

VECTA2(1) = WDX(2) 

VECTA2(2) = WDY(2) 

VECTA2(3) = WDZ(2) 

RAG2(1) = JX2 - LCOGX(2) 

RAG2(2) = JY2 - LCOGY(2) 

RAG2(3) = JZ2 - LCOGZ(2) 

CALL CPROD (VECTA2,RAG2,MIA3,MJA3,MKA3) 

VECTA2(1) = W2(l) 

VECTA2(2) = W2(2) 

VECTA2(3) = W2(3) 

CALL CPROD (VECTA2,RAG2,MIB3,MJB3,MKB3) 

VECTB2(1) = MIB3 
VECTB2(2) = MJB3 
VECTB2(3) = MKB3 

CALL CPROD ( VE CT A2 , VE CTB 2 , MI C3 , M J C3 , MKC3 ) 

AB = AG3 + (WD3 X RB/G3) + W3 X (W3 X RB/G3) 

VECTA2(1) = WDX(3) 

VECTA2(2) = WDY(3) 

VECTA2(3) = WDZ(3) 

RBG3(1) = JX2 - LCOGX(3) 

RBG3(2) = JY2 - LCOGY(3) 

RBG3(3) = JZ2 - LCOGZ(3) 

CALL CPROD (VECTA2,RBG3,MIA4,MKA4,MKA4) 

VECTA2(1) = W3(l) 

VECTA2(2) = W3(2) 

VECTA2(3) = W3(3) 

CALL CPROD (VECTA2,RBG3,MIB4,MJB4,MKB4) 

VECTB2(1) = MIB4 
VECTB2(2) = MJB4 
VECTB2(3) = MKB4 

CALL CPROD (VECTA2,VECTB2,MIC4,MJC4,MKC4) 
SUM OF MOMENTS EQUATIONS 
DO 100 I = 1,3 



COMPUTE HX,HDOT X,HY,HDOT Y, HZ,HDOT Z 



RX(I,1) = -L(I,1) * DCOS(DRCRAX(I)) 
RX(I,2) = L(I,2) * DCOS(DRCRAX(i)) 
RY(I,l) = -L(l,l) * DC0S(DRCRAY(I)) 
RY(I,2) = L(I,2) * DCOS(DRCRAY(l)) 
RZ(l,l) = -L(I,1) * DCOS(DRCRAZ(l)) 
RZ(I,2) = L(I,2) * DCOS(DRCRAZ(l)) 



IXX(I,1) = MASS(I,1) * ((RY(I,1 
IXX(I,2) = MASS(I,2) * ((RY(I,2 
IXXT(I) = IXX(I,1) + IXX(I,2) 
IF (IXXT(I) .LE, .020) THEN 
IXXT(I) = .020 
ELSE 

ixXT(I) = IXXT(I) 

END IF 



* RY(I,1)) + (RZ(I,1) 

* RY(I,2)) + (RZ(I,2) 
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IXY(I,1) = MASS(I,1) * RX(I,i; 
IXY(I,2) = MASS(I,2) * RX(I,2 
IXYT(I) = IXY(I,1) + IXY(I,2; 

* RZ(I,i; 

* RZ(I,2 



IXZ(I,1) = MASS(I,1) 
IXZ(I,2) = MASS(I,2) 



* 

* 

A 

* 



RY(I,1) 

RY(I,2) 



= IXZ(I,1) + IXZ(I,2 



) = WDX(1 


) * IXX< 


;i,i)- wDz(i 


) * IXZ< 


:i,i) 


- WDY(I 


) * 


) = WDX(2 


) * IXX( 


:i,2)- WDZ(I 


) * IXZ< 


[ 1 , 2 ) 


- WDY(I 


) * 



* 



IXZT(I) 

HDX(1 
HDX(2 

IYY(I,1) = MASS(I,1 
IYY(I,2) = MASS(I,2, vv*w*v*. 
lYYT(I) = IYY(I,1) + IYY(I,2) 
IF (lYYT(I) .LE. .020) THEN 
lYYT(I) = .020 
ELSE 

lYYT(l) = lYYT(I) 

END IF 

IYZ(I,1) = MASS(I,1) * RY(I,1) 
IYZ(I,2) = MASS(I,2) * RY(I,2) 
lYZT(I) = IYZ(I,1) + IYZ(I,2) 

HDY(1 
HDY(2 

IZZ(I,1 
IZZ(I,2 
IZZT(I) 

IF (IZZT(I) .LE.’.020) THEN’ 
IZZT(I) = .020 
ELSE 

IZZT(I) = IZZT(I) 

END IF 



RX(i,n 
RX(I,2) 



IXY(I,1 
IXY(I ‘ 



' 2 ! 



* RX(I,1)) + (RZ(I,1) 

* RX(I,2)) + (RZ(I,2) 



* RzSl'J!!! 



* 



RZ(I,1) 

RZ(I,2) 



) = WDY(I 


) * IYY( 


;i,i) 


- WDX(I 


) * IXY( 


;i,i) 


- WDZ(I 


) * IYZ( 


) = WDY(I 


) * IYY( 


\l.2) 


- WDX(I 


) * IXY( 


\l.2) 


- WDZ(I 


) * IYZ( 



Si'z! 



) = MASS(I,1) 


* ((RX< 


;i,i) 


* RX( 




) + (RY( 


;i,i) 


* RY( 




) = MASS(I,2) 


* 


\l,2) 


* RX( 


\l. 2 y, 


) + (RY( 


\l.2) 


* RY( 


\l.2)) 



(1) = WDZfl 


) * IZZ( 


;i,i) 


.- WDX(I 


) * IXZ( 


;i,i) 


- WDY(I 


) * IYZ( 


;i,i) 


(2) = WDZ(I 


) * IZZ( 


[ 1 , 2 ) 


- WDX(I 


) * IXZ( 


[ 1 , 2 ) 


- WDY(I 


) * IYZ( 


[ 1 , 2 ) 



SUMHDX(I) = HDX(l) + HDX( 

SUMHDY(I) = HDYQ) + HDY 

SUMHDZ{I) = HDZ(l) + HDZ( 

CONTINUE 

ENTER CONSTANTS INTO MATRIX A 
LINK ONE 

SUM OF FORCES IN THE X DIRECTION 

MATA(1,1) = 

MATA(1,4) = 



l.ODO 

MASSl 



MATA(1,10) = -l.ODO 

SUM OF FORCES IN Y DIRECTION 

MATA(2,2) = l.ODO 

MATA(2,5) = MASSl 

MATA(2,11) = -l.ODO 

SUM OF FORCES IN Z DIRECTION 

MATA(3,3) = l.ODO 

MATA(3,6) = MASSl 

MATA(3,12) = -l.ODO 

EQUATIONS AT JOINT ZERO 
IN THE X DIRECTION 

MATA(4,4) = l.ODO 
MATA(4,8) = RBG1(3) 

MATA(4,9) = -RBG1(2) 

IN THE Y DIRECTION 

MATA(5,5) = l.ODO 
MATA(5,7) = -RBG1(3) 
MATA(5,9) = RBGl(l) 
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* 






* 



IN THE Z DIRECTION 



MATA( 


6,6) 


= l.ODO 


MATA< 


6,7) 


= RBG1(2) 


MATA( 


6,8) 


= -RBGl(l) 



SUM OF MOMENTS EQUATIONS 



MATA< 


7,2) 




RBGK 


[3) 


MATA( 


7,3) 


= 


-RBGK 


2) 


MATA< 


7,7) 


= 


-IXXT( 


l) 


MATAI 


7,8) 




IXYT< 


1) 


MATA< 


7,9)^ 


= 


IXZTi 


l) 


MATA< 


7,11) 


= 


-RAGK 


3) 


MATAI 


[7,12) 


=: 


RAGK 


2) 



FOR LINK ONE IN THE X, 



MATA(8,1) 

MATA(8,3) 

MATA(8,7) 

MATA(8,8) 

MATA(8,9) 

MATA(8,10) 

MATA(8,12) 



-RBG1(3) 

RBGl(l) 

IXYT(l) 

-IYYT(l) 

lYZT(l) 

RAG1(3) 

-RAGl(l) 



MATA< 


;9,i) 


MATAI 


9,2) 


MATAi 


9,7) 


MATA( 


9,8) 


MATA< 


9,9) 


MATA( 


9,10 


MATA< 


9,11 



= RBG1(2) 
= -RBGl(l) 
= IXZT(l) 
= lYZT(l) 
= -IZZT(l) 
= -RAG1(2) 
= RAGl(l) 



+ IXZT(2) 
+ IYZT(2) 
- IZZT(2) 



+ IXZT(3) 
+ IYZT(3) 
- IZZT(3) 



LINK TWO 



SUM OF FORCES IN X DIRECTION 

MATA(10,10) = l.ODO 
MATA(10,13) = MASS2 
MATA(10,19) = -l.ODO 

SUM OF FORCES IN THE Y DIRECTION 

MATA(11,11) = l.ODO 
MATA(11,14) = MASS2 
MATA(11,20) = -l.ODO 

SUM OF FORCES IN THE Z DIRECTION 

MATA(12,12) = l.ODO 
MATA(12,15) = MASS2 
MATA(12,21) = -l.ODO 

EQUATIONS AT JOINT ONE 

IN THE X DIRECTION 

MATA(13,4) = -l.ODO 

MATA(13,8) = -RAG1(3) 

MATA(13,9) = RAG1(2) 

MATA(13,13) = l.ODO 
MATA(13,17) = RBG2(3) 

MATA(13,18) = -RBG2(2) 

IN THE Y DIRECTION 

MATA(14,5) = -l.ODO 

MATA(14,7) = RAG1(3) 

MATA(14,9) = -RAGl(l) 

MATA(14,14) = l.ODO 
MATA(14,16) = -RBG2(3) 
HATA(14,18) = RBG2(1) 

IN THE Z DIRECTION 

MATA(15,6) = -l.ODO 

MATA(15,7) = -RAG1(2) 

MATA(15,8) = RAGl(l) 

MATA(15,15) = l.ODO 
MATA(15,16) = RBG2U) 



,Z DIRECTIONS 
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MATA(15,17) = -RBG2(1) 

SUM OF MOMENTS EQUATIONS FOR LINK TWO IN THE X,Y,Z DIRECTIONS 



MATAI 


[16,11) 


= 


RBG2I 


[3) 








MATAI 


16,12) 


= 


-RBG2 1 


2) 








MATAI 


16,16) 


= 


-IXXTI 


2) 








MATAI 


16,17) 


= 


IXYTI 


2) 








MATAI 


16,18) 


= 


IXZTI 


2) 








MATAI 


16, 20) 


rr 


-RAG2I 


3) 








MATAI 


[16, 2l) 


= 


RAG2I 


[2) 








MATAI 


[17,10) 


= 


-RBG2I 


[3) 








MATAI 


17,12) 


= 


RBG2I 


l) 








MATAI 


17,16) 


= 


IXYTI 


2) 








MATAI 


17,17) 


= 


-lYYTl 


2) 








MATAI 


17,18) 


= 


lYZTl 


2) 








MATAI 


17,19) 


= 


RAG2I 


3) 








MATAI 


[17, 2l) 




-RAG2I 


[l) 








MATAI 


[18,10) 


= 


RBG2I 


[2) 








MATAI 


18, ll) 


= 


-RBG2I 


l) 








MATAI 


18,16) 


= 


IXZTI 


2) 




IXZTI 


[3 


MATAI 


18,17) 


= 


lYZTl 


2) 


+ 


lYZTI 


3 


MATAI 


18,18) 


= 


-IZZTI 


2) 


- 


IZZTI 


3 


MATAI 


18,19) 


= 


-RAG2 1 


2) 








MATAI 


18, 20) 


= 


RAG2I 


[l) 









LINK THREE 

SUM OF FORCES IN THE X DIRECTION 

MATA(19,19) = l.ODO 
MATA (19, 22) = MASS3 

SUM OF FORCES IN THE Y DIRECTION 

MATA(20,20) = l.ODO 
MATA(20,23) = MASS3 

SUM OF FORCES IN THE Z DIRECTION 

MATA(21,21) = l.ODO 
MATA (21, 24) = MASS3 

EQUATIONS AT JOINT TWO 

IN THE X DIRECTION 



MATAI 


[22,13) 


= 


-l.ODO 


MATAI 


22,17) 


= 


-RAG2(3) 


MATAI 


22,18) 


= 


RAG2(2) 


MATAI 


22,22) 


= 


l.ODO 


MATAI 


22,26) 




RBG3(3) 


MATAI 


22,27) 


= 


-RBG3(2) 



IN THE Y DIRECTION 



MATAI 


[23,14) 


= 


-l.ODO 


MATAI 


23,16) 


= 


RAG2(3) 


MATAI 


23,18) 


= 


-RAG2(l) 


MATAI 


23,23) 


= 


l.ODO 


MATAI 


23,25) 


= 


-RBG3(3) 


MATAI 


23,27) 


= 


RBG3 ( 1 ) 



IN THE Z DIRECTION 



MATAI 


[24,15) 


= 


-l.ODO 


MATAI 


24,16) 


= 


-RAG2(2) 


MATAI 


24,17) 


= 


RAG2(l) 


MATAI 


24,24) 


= 


l.ODO 


MATAI 


24,25) 


= 


RBG3(2) 


MATAI 


[24,26) 


= 


-RBG3(l) 



SUM OF MOMENTS EQUATIONS FOR LINK THREE IN THE X,Y,Z DIRECTIONS 

MATA(25,20) = RBG3(3) 

MATA(25,2l) = -RBG3(2) 

MATA(25,25) = -IXXT(3) 
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MATA(25,26j 



X 

* 

'k 



X 

X 

X 

•k 

X 

X 

k 

k 



MATA (25, 27 

MATA 
MATA 
MATA 
MATA 
MATA 

MATA( 

MATA< 

MATA 
MATA 
MATA 



IXYT(3j 



ixzt(3; 

[26.19) = -RBG3(3; 

26,21) = RBG3(1 

26.25) = IXYT(3 

26.26) = -IYYT(3 

[26.27) = IYZT(3; 

[27.19) = RBG3(2] 

27,20) = -RBG3(1 

27.25) = IXZT(3 

27.26) = IYZT(3 

[27.27) = -IZZT(3; 

GO TO 1112 

INITIALIZE MATRIX ACCORDING TO CONSTRAINT 
CONSTRAINTS GROUP 1 WHEN ONLY LINK THREE IS IN MOTION 



3^ 


DO 18 J = 1, 


,27 


k 


MATA( 


[I,J 


= 0.0 


k 


MATAI 


I/I 


= 1.0 


k 


MATB( 


I 


= 0.0 


^18 

*118 

X 


CONTINUE 

CONTINUE 




X 


DO 181 I 


= 19,27 


X 


DO 81 J = 1, 


,18 


k 


MATA(I,J) 


= 0.0 


*81 


CONTINUE 




*181 


CONTINUE 




k 


GO TO 1111 




k 


CONSTRAINTS GROUP 2 WHEN : 


k 


DO 19 I = 


= 1,9 




k 


DO 191 


J = 1,27 


X 


MATAI 


I/J) 


= O.ODO 


X 


MATAI 




= 1.0 DO 


k 

X 


MATBI 


= O.ODO 


k 


MATAI 


[17, j; 


1 = O.ODO 


k 


MATAI 


18, J] 


) = O.ODO 


X 


MATBI 


17^ 


= O.ODO 


X 

k 


MATBI 


[l8) 


= O.DO 


k 


MATAI 


[j/17; 


» = O.ODO 


k 

k 


MATAI 




1 = O.ODO 


X 


MATAI 


[17,17) = l.ODO 


k 

k 


MATAI 


[l8,18) = l.ODO 


*191 

*19 

X 


CONTINUE 

CONTINUE 




k 


DO 91 I = 


= 10,27 


k 


DO 92 J = 1, 


,9 


X 


MATA(I,J) 


= 0.0 


*92 

*91 


CONTINUE 

CONTINUE 




X 


GO TO 1111 





WHEN LINK TWO AND THREE ARE IN MOTION 



CONSTRAINTS GROUP 3 WHEN THREE OF THE LINKS ARE IN MOTION 



DO 78 J = 1,27 


HATA( 


[7,J 


1 = O.ODO 


MATA i 


8,J 


1 = O.ODO 


MATA( 




1 = O.ODO 


MATAI 


J,8 


1 = O.ODO 


MATS ( 


7 


= O.ODO 


MATBI 


8 


= O.ODO 
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k 


MATA( 


17, J) 


= 


O.ODO 


k 


MATA( 


18, J) 


= 


O.ODO 


k 


MATA< 


J,17) 




O.ODO 


k 


MATA< 


J,18) 


= 


O.ODO 


k 


MATB( 


17) 


= 


O.ODO 


k 


MATB< 


Jl8) 




O.ODO 


k 


MATAI 


[26, J) 


= 


O.ODO 


k 


MATA< 


27, J) 


sr 


O.ODO 


k 


MATA< 


J/26) 


s 


O.ODO 


k 


MATA< 


J,27) 


= 


O.ODO 


k 


MATBI 


26) 


= 


O.ODO 


k 


MATB< 


1 27) 




O.ODO 


k 


MATA< 


;7,7) 




= l.ODO 


k 


MATA( 


8,8) 




= l.ODO 


k 


MATAI 


17,17 




= l.ODO 


k 


MATA( 


18,18 




= l.ODO 


k 


MATAI 


26,26 




= l.ODO 


k 


MATAI 


27,27 




= l.ODO 



*78 CONTINUE 



* CONSTRAINT GROUP 4 THE FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIR. 



1112 DO 48 I = 4,8 

DO 481 J = 1,27 

MATA(I,J) = O.ODO 
MATA(I,I) = 1.0 DO 
MATB(I) = O.ODO 
481 CONTINUE 

48 CONTINUE 

DO 84 I = 9,27 
DO 841 J = 4,8 

MATA(I,J) = 0.0 ■ 

841 CONTINUE 

84 CONTINUE 

* USE THE FOLLOWING SET OF INFORMATION WHEN THE ANGULAR VELOCITY IS 

* IN X DIRECTION REGARDLESS OF THE INITIAL CONFIGURATION 



* ENTER THE THEORITICAL VALUES ASSUMING THE LINK TWO AND THREE ARE 

* IN PLANAR MOTION AND ANGULAR VELOCITY IS IN THE X DIRECTION 



* 

•k 



k 



k 

k 



k 

k 



k 

k 



LINK TWO 

THEORITICAL ANGULAR VELOCITIES (APPLIED IN THE X DIRECTION) 

MATB(18) = O.ODO 
MATBa7) = O.ODO 

MATB(16) = -((PI**3) / 8.0D0) * DSIN(W * TIME) 

THDDOT = MATB(16) 

THTDOT = INTGRL((PI**2)/4. , THDDOT) 

THETRB = INTGRL(0. , THTDOT ) 

THETAB = THETRB * RADEG ' 

LINEAR VELOCITIES 

MATB(15) = -(THDDOT * RBG2(2)) + (THTDOT ** 2) * RBG2(3) 
MATB(14) = (THDDOT * RBG2(3)) + (THTDOT ** 2) * RBG2(2) 
MATB(13) = O.ODO 

LINK THREE 
ANGULAR VELOCITIES 

MATB(27) = O.ODO 
MATB(26) = O.ODO 
MATB(25) = O.ODO 

LINEAR VELOCITIES 

MATB(24) = MATB(15)+(THDD0T*RAG2(2))-(THTD0T**2)*(RAG2(3)) 
MATB(23) = MATB(14)-(tHDDOT*RAG2(3))-(THTDOT**2)*(RAG2(2)) 
MATB(22) = MATB(13) 

END OF THE INFORMATION FOR X DIRECTION 
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* USE THE FOLLOWING SET OF INFORMATION WHEN THE ANGULAR VELOCITY IS 

* IN THE Y DIRECTION REGARDLESS OF THE INITIAL CONFIGURATION 

* ENTER THE THEORITICAL VALUES ASSUMING THE LINK TWO AND THREE ARE 

* IN PLANAR MOTION AND ANGULAR VELOCITY IS IN THE Y DIRECTION 



* 

* 

* 

* 

A 

* 

A 

* 

* 

A 

* 

A 

* 

■k 

k 



LINK TWO 

THEORITICAL ANGULAR VELOCITIES (APPLIED IN THE Y DIRECTION ) 



MATB(18) = 
MATB(17) = 
MATB(16) = 
THDDOT 
THTDOT 
THETRB = 
THE TAB = 

LINEAR VELOCITIES 



O.ODO 

((-PI**3)/8)*SIN(W*TIME) 

O.ODO 

MATBU7) 

INTGRL((PI**2)/4. , THDDOT) 

INTGRL(0 , INTGRL (PI**2/4 . , THDDOT) ) 
THETRB * RADEG 



MATE (15) 
MATE (14) 
MATE (13) 



(THDDOT * RBG2(1)) + 
O.ODO 

-(THDDOT * RBG2(3)) + 



(THTDOT ** 2) 
(THTDOT ** 2) 



LINK THREE 
ANGULAR VELOCITIES 



* RBG2(3) 

* RBG2(1) 



* MATB(27) = O.ODO 

* MATB(26) = O.ODO 

* MATB(25) = O.ODO 

LINEAR VELOCITIES 



* MATB(24) = MATB(15)-(THDD0T*RAG2(1))-(THTD0T**2)*(RAG2(3)) 
MATB(23) = MATB(14) 

* MATB(22) = MATB(13)+(THDDOT*RAG2(3))-(THTDOT**2)*(RAG2(l)) 

* END OF THE INFORMATION FOR THE Y DIRECTION 



* USE THE FOLLOWING SET OF INFORMATION WHEN THE ANGULAR VELOCITY IS 

* IN THE Z DIRECTION REGARDLESS OF THE INITIAL CONFIGURATION 

* ENTER THE THEORITICAL VALUES ASSUMING THE LINK TWO AND THREE ARE 

* IN PLANAR MOTION AND ANGULAR VELOCITY IS IN THE Z DIRECTION 



* LINK TWO 

« THEORITICAL ANGULAR VELOCITIES (APPLIED IN THE Z DIRECTION ) 

* MATB(16) = O.ODO 

« MATB(17) = O.ODO 

* MATB(18) = -((PI**3) / 8.0D0) * DSIN(W * TIME) 

* THDDOT = MATE (18) 

* THTDOT = INTGRL((PI**2)/4. , THDDOT) 

* THETRB = INTGRL (0. .THTDOT) 

* THETAB = THETRB * RADEG 

* LINEAR VELOCITIES 



MATB(14) = -(THDDOT * RBG2(1)) + (THTDOT ** 2) * RBG2(2 

* MATB(13) = (THDDOT * RBG2(2)) + (THTDOT ** 2) * RBG2(l 

* MATB(15) = O.ODO 

* LINK THREE 

* ANGULAR VELOCITIES 

k 

* MATB(27) = O.ODO 

* MATB(26) = O.ODO 

* MATB(25) = O.ODO 

* LINEAR VELOCITIES 



* MATB(24) = MATB(15) 

* MATB(23) = MATB(14)+(THDDOT*RAG2(l))-(THTDOT**2)*(RAG2(2)) 

* MATB(22) = MATB(13)-(THDDOT*RAG2(2))-(THTDOT**2)*(RAG2(l)) 

* END OF THE INFORMATION FOR THE Z DIRECTION 



* NEXT SET OF STATEMENTS ARE COMMON IN ANY PLANAR MOTION AND THEY ARE 

* IN YHE CODE IN EVERY CASE. THESE TERMS ARE ACCELERATION OF THE LINK 
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:k 

* 



ONE AND FORCES AT EACH JOINT 
LINK ONE LINEAR AND ANGULAR ACCELERATIONS 



MATE! 


(4) 


= 


O.ODO 


MATE! 


5) 


=: 


O.ODO 


MATE! 


6) 


= 


O.ODO 


MATE! 


7) 


= 


O.ODO 


MATEI 


8) 


= 


O.ODO 


MATE! 


9) 


= 


O.ODO 



* 



FORCES 

JOINT 



TWO 

MATE (21 
MATE (20 
MATE 



19) = 



-MASS 3 
-MASS3 
-MASS3 



* 

* 

* 



MATE (24) 
MATE! 
MATB< 



JOINT ONE 

MATE 
MATE 



(12) 


= MATE< 


;21) 


- MASS2 




(11) 


= MATE< 


20) 


- MASS2 


■k 


(10) 


= MATE< 


19) 


- MASS2 


k 




* 

* 



MATE (12 

mate(ii 

MATE (10 

END OF THE INFORMATION 

MULTIPLY MATA AND MATE 

DO 505 J = 1,27 
SUMl =0.0 

= 1,27 



- MASSl 

- MASSl 

- MASSl 



WG3 



MATE( 

MATE( 

MATE( 



MATE( 

MATE< 

MATE( 



-WG2 



-WGl 



555 

505 

506 



* 

* 

* 



DO 555 K 

SUMl = SUM! 
CONTINUE 
DQ(J) = SUMl 



+ MATA(J,K) * MATE(K) 



CONTINUE 

DO 506 I =1,27 

MATC(I) = DQ(I) 

CONTINUE 

CALL EQUATION SOLVER PROGRAM FROM IMSL 
CALL LEQT2F ( MATA , M , N , I A , DQ , IDGT , WKAREA , lER) 
IF (lER .NE. 0) CALL ENDJOE 
FIND LCOGK,LCOGY,LCOGZ, THETA VALUES ,WX,WY,WZ 
LINK ONE 



AXl 

VELXl 

LCOGXl 

LCOGX(l) 

AYl 

VELYl 

LCOGYl 

LCOGY(l) 

AZl 

VELZl 

LCOGZl 

LCOGZ(l) 

WDIX 

WIX 

WDX(l) 

Wl(l) 

WDIY 

WIY 

WDY(l) 

Wl(2) 



= DQ(4) 

= INTGRL(0. ,AX1) . 

= INTGRL( XI, VELXl) 
= LCOGXl 



= DQ(5) 
= INTr~ 



)1TGRL(0. ,AY1) 

= INTGRL(Y1, VELYl) 
= LCOGYl 



= DQ(6) 
= INTG“ 



_ TGRL(0. ,AZ1) 
INTGRL(Z1, VELZl) 
= LCOGZl 

= DQ(7) ^ 

= INTGRL(0. ,WD1X) 

= WDIX 
= WIX 

= DQ(8) 

= INTGRL(0. ,WD1Y) 

= WDIY 
= WIY 
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606 
605 



WDIZ = DQ(9) 

WIZ = INTGRL(0. ,WD1Z) 

WDZ(l) = WDIZ 

Wl(3) = WIZ 



TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
SYSTEM FOR THE LINK ONE 



MAT1R(1,1) = DCOS(RLRZl) * 

MAT1R(2,1) = DCOS(RLRZl) * 
DSIN(RLRZl) * DCOS(YWRXl) 

MAT1R(3,1) = DCOS(RLRZl) * 
DSIN(RLRZl) * DSIN(YWRXl) 

MAT1R(1,2) = DSIN(RLRZl) * 

MAT1R(2,2) = DSIN(RLRZl) * 
DCOS(RLRZl) * DCOS(YWRXl) 

MAT1R(3,2) = DSIN(RLRZl) * 
DCOS(RLRZl) * DSIN(YWRXl) 

MAT1R(1,3) = -DSIN(PTRYl) 

MAT1R(2,3) = DCOS(PTRYl) * 

MAT1R(3,3) = DCOS(PTRYl) * 

GET THE VELOCITIES FOR LINK 1 



DCOS(PTRYl) 

DSIN(PTRYl) * DSIN(YWRXl) 
DSIN(PTRYl) * DCOS(YWRXl) +. 
DCOS(PTRYl) 

DSIN(PTRYl) * DSIN(YWRXl) +. 
DSIN(PTRYl) * DCOS(YWRXl) 

DSIN(YWRXl) 

DCOS (YWRXl ) 

IN BODY FIXED COOR. SYSTEM 



DO 605 J = 1,3 
SUMl = O.ODO 
DO 606 K = 1,3 

SUMl = SUMl + MAT1R(J,K) * W1(K) 
CONTINUE 

BRATEl(J) = SUMl 
CONTINUE 



TRANSFORMATION MATRIX FROM BODY FIXED TO EULER COORDINATE 
SYSTEM FOR THE LINK ONE 

MAT1T(1,1) = O.ODO 
MAT1T(2,1) = l.ODO 
MAT1T(3,1) = O.ODO 

MAT1T(1,2) = DCOS (YWRXl) 

MAT1T(2,2) = DTAN(PTRYl) * DSIN(YWRXl) 

MAT1T(3,2) = 1.0DO/DCOS(PTRY1) ^ DSIN(YWRXl) 

MAT1T(1,3) = -DSIN(YWRXl) 

MAT1T(2,3) = DTAN(PTRYl) * DCOS (YWRXl) 

MAT1T(3,3) = l.DO/DCOS(PTRYl) * DCOS(YWRXI) 

GET THE YAW, PITCH AND THE ROLL RATES FOR LINK ONE 

DO 705 J = 1,3 
SUMl = O.ODO 
DO 706 K = 1,3 

SUMl = SUMl + MAT1T(J,K) * BRATEl(K) 

706 CONTINUE 

RATEl(J) = SUMl 
705 CONTINUE 

RATEIX = RATEl(l) 

RATEIY = RATE1(2) 

RATEIZ = RATE1(3) 

YWRXl = INTGRL(0. , RATEIX) 

PTRYl = INTGRL(0. , RATEIY) 

RLRZl = INTGRL(0. , RATEIZ ) 

YAWANX(l) = YWRXl * RADEG 
PTCANY(i) = PTRYl * RADEG 
ROLANZ(l) = RLRZl * RADEG 

DIRECTION COSINES FOR THE FIRST LINK SAME FOR EACH CASE 

DRCSY(l) = DCOS(RLRZl) * DSIN(PTRYl) * DCOS(YWRXl) +... 
DSIN(RLRZl) * DSIN(YWRXl) 
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DRCSX(l) = DSIN(RLRZl) * DSIN(PTRYl) * DC0S{YWRX1) 
DCOS(RLRZl) * DSIN(YWRXl) 

DRCSZ(l) = DCOS(P.TRYl) * DCOS(YWRXl) 

GET THE ANGLES AS RADIANS 

DRCRAX(l) = DACOS(DRCSX(l)) 

DRCRAY(l) = DACOS(DRCSY(l)) 

DRCRAZ(l) = DACOS(DRCSZ(l)) 

CONVERT THE DIRECTION COSINES TO DEGREES 

DRCANX(l) = DACOS(DRCSX(l)) * RADEG 

DRCANY(l) = DACOS(DRCSY(l)) * RADEG 

DRCANZ(l) = DACOS(DRCSZ(l)) * RADEG 

LINK TWO 

AX2 = DQ(13) 

VELX2 = INTGRL(0. ,AX2) 

LCOGX2 = INTGRL(X2,VELX2) 

LCOGX(2> = LCOGX2 

AY2 = DQ(14) 

VELY2 = INTGRL(0. ,AY2) 

LCOGY2 = INTGRL(Y2,VELY2) 

LC0GY(2) = LCOGY2 

AZ2 = DQ(15) 

VELZ2 = INTGRL(0. ,AZ2) 

LCOGZ2 = INTGRL(Z2,VELZ2) 

LCOGZ(2) = LCOGZ2 

WD2X = DO (16) 

W2X = INTGRL((PI**2)/4. ,WD2X) 

USE THE INIT. COND. WITH ONLY WHICHEVER VELOCITY APPLIED 
AND KEEP THE TWO OTHER ANG. VEL. INIT. COND. AS ZERO 



USE THE NEXT STATEMENT IF THE ANGULAR VELOCITY IS IN THE X DIR. 
THETRD = INTGRL(0. ,W2X) 

WDX(2) = WD2X 

W2(l) = W2X 

WD2Y = DO (17) 

W2Y = INTGRL(0. ,WD2Y) 

USE THE NEXT STATEMENT IF THE ANGULAR VELOCITY IS IN THE Y DIR. 
THETRD = INTGRL(0. ,W2Y) 

WDY(2) = WD2Y 

W2(2) = W2Y 

WD2Z = DO (18) 

W2Z = INTGRL(0. ,WD2Z) 

USE THE NEXT STATEMENT IF THE ANGULAR VELOCITY IS IN THE Z DIR. 



THETRD = INTGRL(0. ,W2Z) 

WDZ(2) = WD2Z 

W2(3) = W2Z 

THETAD = THETRD * RADEG 

ERROR = ABS(((THETAD-THETAB)/180.) * 100) 

TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COORDINATE 
SYSTEM FOR THE LINK TWO 



MAT2R(1,1) = DCOS(RLRZ2) * DCOS(PTRY2) 

MAT2R(2,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) -... 
DSIN(RLRZ2) * DCOS(YWRX2) 

MAT2R(3,1) = DCOS(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) +... 
DSIN(RLRZ2) * DSIN(YWRX2) 

MAT2R(1,2) = DSIN(RLRZ2) * DCOS(PTRY2) 

MAT2R(2,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) +... 
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DC0S(RLRZ2) * DC0S(YWRX2) 

MAT2R(3,2) = DSIN(RLRZ2) * DSIN(PTRY2) * DC0S(YWRX2) 
DC0S(RLRZ2) * DSIN(YWRX2) 

MAT2R(1,3) = -DSIN(PTRY2) 

MAT2R(2,3) = DC0S(PTRY2) * DSIN(YWRX2) 

MAT2R(3,3) = DC0S(PTRY2) * DC0S(YWRX2) 

GET THE VELOCITIES FOR LINK 2 IN BODY FIXED COOR. SYSTEM 

DO 607 J = 1,3 
SUMl = O.ODO 
DO 608 K = 1,3 

SUMl = SUMl + MAT2R(J,K) * W2(K) 

608 CONTINUE 

BRATE2(J) = SUMl 
607 CONTINUE 

TRANSFORMATION MATRIX FROM BODY FIXED TO EULER COOR. SYSTEM 
FOR THE LINK TWO 

MAT2T(1,1) = O.ODO 
MAT2T(2,1) = l.ODO 
MAT2T(3,1) = O.ODO 

MAT2T(1,2) = DCOS(YWRX2) 

MAT2T(2,2) = DTAN(PTRY2) * DSIN(YWRX2) 

MAT2T(3,2) = 1 . ODO/DCOS (PTRY2 ) * DSIN(YWRX2) 

MAT2T(1,3) = -DSIN(YWRX2) 

MAT2T(2,3) = DTAN(PTRY2) * DCOS(YWRX2) 

MAT2T(3,3) = 1 .ODO/DCOS(PTRY2) * DCOS(YWRX2) 

GET THE YAW, PITCH AND THE ROLL RATES FOR LINK TWO 

DO 707 J = 1,3 
SUMl = O.ODO 
DO 708 K = 1,3 

SUMl = SUMl + MAT2T(J,K) * BRATE2(K) 

708 CONTINUE 

RATE2(J) = SUMl 
707 CONTINUE 

RATE2X = RATE2(1) 

RATE2Y = RATE2(2) 

RATE2Z = RATE2(3) 

USE THE NEXT THREE STATEMENTS FOR CASE A 

Y>mX2 = INTGRL(0. ,RATE2X) 

PTRY2 = INTGRL(0. ,RATE2Y) 

RLRZ2 = INTGRL(-PI/2. ,RATE2Z) 

USE THE NEXT THREE STATEMENTS FOR CASE B 

YWRX2 = INTGRL(0. ,RATE2X) 

PTRY2 = INTGRLCO. ,RATE2Y) 

RLRZ2 = INTGRL(PI/2. ,RATE2Z) 

USE THE NEXT THREE STATEMENTS FOR CASE C 

YV;RX2 = INTGRL(0. ,RATE2X) 

PTRY2 = INTGRL(0. ,RATE2Y) 

RLRZ2 = INTGRL(0. ,RATE2Z) 

RATE2(1) = RATE2X 
RATE2(2) = RATE2Y 
RATE2(3) = RATE2Z 

YAWANX(2) = YWRX2 * RADEG 
PTCANY(2) = PTRY2 * RADEG 
ROLAI'IZ(2) = RLRZ2 * RADEG 

USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK TWO FOR CASE A 

DRCSY(2) = DCOS(RLRZ2) * DSIN(PTRY2) * DSIN(YWRX2) -... 
DSIN(RLRZ2) * DCOS(YWRX2) 

DRCSX(2) = DSIN(RLRZ2) * DSIN(-PTRY2) * DSIN(YWRX2) +... 
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DC0S(RLRZ2) * DC0S(YWRX2) 

DRCSZ(2) = DC0S(PTRY2) * DSIN(YWRX2) 

USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK TWO FOR CASE B 



DRCSY(2) = DCOS(RLRZ2) * DCOS(PTRY2) 

DRCSX(2) = DSIN(RLRZ2)*DCOS(PTRY2) 

DRCSZ(2) = -DSIN(PTRY2) 

USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK TWO FOR CASE C 

DRCSY(2) = DCOS(RLRZ2) * DSIN(PTRY2) * DC0S(YWRX2) +... 
DSIN(RLRZ2) * DSIN(YWRX2) 



DRCSX(2) = DSIN(RLRZ2) * DSIN(PTRY2) * DCOS(YWRX2) 
DCOS{RLRZ2) * DSIN(YWRX2) 

DRCSZ(2) = DCOS(PTRY2) * DC0S(YWRX2) 

GET THE ANGLES AS RADIANS 

DRCRAX(2) = DACOS(DRCSX(2)) 

DRCRAY(2) = DACOS(DRCSY(2)) 

DRCRAZ(2) = DACOS(DRCSZ(2)) 

CONVERT THE DIRECTION COSINES TO DEGREES 

DRCANX(2) = DACOS(DRCSX(2>) * RADEG 

DRCANY(2) = DACOS(DRCSY(2)) * RADEG 

DRCANZ(2) = DACOS(DRCSZ(2)) * RADEG 

FIND THE JOINT LOCATION 

JXl = (L(l,l) + L(l,2)) * DCOS(DRCRAX(l)) 

JYl = (L(l,l) + L(l,2)) * DCOS(DRCRAY(l) ) 

JZl = (L(l,l) + L(l,2)) * DCOS(DRCRAZ(l)) 

LINK THREE 



AX3 = DQ(22) 

VELX3 = INTGRL(0. ,AX3) 
LCOGX3 = INTGRL(X3,VELX3) 
LCOGX{3) = LCOGX3 



AY3 

VELY3 

LCOGY3 

LCOGY{3) 



DQ(23) 

INTGRL(0. ,AY3) 

INTGRL(Y3,VELY3) 

LCOGY3 



AZ3 

VELZ3 

LCOGZ3 

LCOGZ{3) 



DQ(24) 

INTGRL(0. ,AZ3) 

INTGRL(Z3,VELZ3) 

LCOGZ3 



WD3X = DQ(25) 

W3X = INTGRL(0. ,WD3X) 

WDX(3) = WD3X 

W3(l) = W3X 



WD3Y = DO (26) 

W3Y = INTGRL(0. ,WD3Y) 

WDY(3) = WD3Y 

W3(2) = W3Y 



WD3Z 

W3Z 

WDZ(3) 

W3(3) 



DQ(27)^ 

INTGRL(0. ,WD3Z) 

WD3Z 

W3Z 



TRANSFORMATION MATRIX FROM EARTH FIXED TO BODY FIXED COOR. 
FOR THE LINK THREE 



SYSTEM 



MAT3R(1,1) = DCOS(RLRZ3) * DCOS(PTRY3) 

MAT3R(2,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) 
DSIN(RLRZ3) * DCOS(YWRX3) 

MAT3R(3,1) = DCOS(RLRZ3) * DSIN(PTRY3) * DCOS(YWRX3) +... 
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DSIN(RLRZ3) * DSIN(YWRX3) 

MAT3R(1,2) = DSIN(RLRZ3) * DC0S(PTRY3) 

MAT3R(2,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) +... 
DC0S(RLRZ3) * DC0S(YWRX3) 

MAT3R(3,2) = DSIN(RLRZ3) * DSIN(PTRY3) * DC0S(YWRX3) 
DC0S(RLRZ3) * DSIN(YWRX3) 

MAT3R(1,3) = -DSIN(PTRY3) 

MAT3R(2,3) = DC0S(PTRY3) * DSIN(YWRX3) 

MAT3R(3,3) = DC0S(PTRY3) * DC0S(YWRX3) 

GET THE VELOCITIES FOR LINK 3 IN BODY FIXED COOR. SYSTEM 

DO 609 J = 1,3 
SUMl = O.ODO 
DO 610 K = 1,3 

SUMl = SUMl + MAT3R(J,K) * W3(K) 

610 CONTINUE 

BRATE3(J) = SUMl 
609 CONTINUE 

TRANSFORMATION MATRIX FROM BODY FIXED TO EULER COOR. SYSTEM 
FOR THE LINK THREE 

MAT3T(1,1) = O.ODO 
MAT3T(2,1) = l.ODO 
MAT3T(3,1) = O.ODO 

MAT3T(1,2) = DC0S(YWRX3) 

MAT3T(2,2) = DTAN(PTRY3) * DSIN{YWRX3) 

MAT3T(3,2) = 1 . ODO/DCOS (PTRY3 ) DSIN(YWRX3) 

MAT3T(1,3) = -DSIN(YWRX3) 

MAT3T(2,3) = DTAN(PTRY3) * DC0S(YWRX3) 

MAT3T(3,3) = 1 .ODO/DCOS (PTRY3) * DC0S(YWRX3) 

GET THE YAW, PITCH AND THE ROLL RATES FOR LINK THREE 

DO 709 J = 1,3 
SUMl = O.ODO 
DO 710 K = 1,3 

SUMl = SUMl + MAT3T(J,K) * BRATE3(K) 

710 CONTINUE 

RATE3(J) = SUMl 
709 CONTINUE 

RATE3X = RATE3(1) 

RATE3Y = RATE3(2) 

RATE3Z = RATE3(3) 

USE THE NEXT THREE FOR THE CASE A 

YWRX3 = INTGRL(0. ,RATE3X) 

PTRY3 = INTGRL(0. ,RATE3Y) 

RLRZ3 = INTGRL(-PI/2. ,RATE3Z) 

USE THE NEXT THREE FOR THE CASE B 

YWRX3 = INTGRL(0. ,RATE3X) 

PTRY3 = INTGRLtO. ,RATE3Y) 

RLRZ3 = INTGRL(PI/2. ,RATE3Z) 

USE THE NEXT THREE FOR THE CASE C 

YWRX3 = INTGRL(0. ,RATE3X) 

PTRY3 = INTGRL(0. ,RATE3Y) 

RLRZ3 = INTGRL(0. ,RATE3Z) 

YAWANX(3) = YWRX3 * RADEG 
PTCANY(3) = PTRY3 * RADEG 
R0LANZ(3) = RLRZ3 * RADEG 

USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK THREE FOR CASE A 

DRCSY(3) = DCOS(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3) -... 
DSIN(RLRZ3) * DCOS(YWRX3) 



99 



> yh yh yh ^ ^ yh A- ^ yh ^ yh yh y^ ^ yh 



DRCSX(3) = DSIN(RLRZ3) * DSIN(PTRY3) * DSIN(YWRX3)+ . . . 
DC0S(RLRZ3) * DC0S(YWRX3) 

DRCSZ(3) = DC0S(PTRY3) * DSIN(YWRX3) 

USE THE NEXT SET OF THE DIRECTION COSINES FOR LINK THREE FOR CASE B 
DRCSY(3) = DCOS{RLRZ3) * DCOS(PTRY3) 

DRCSX(3) = DSIN(RLRZ3)*DCOS(PTRY3) 



FOR LINK THREE FOR CASE C 
* DCOS(YWRX3)+. . . 



DRCSZ(3) = -DSIN(PTRY3) 

USE THE NEXT SET OF THE DIRECTION COSINES 

DRCSY(3) = DCOS(RLRZ3) * DSIN(PTRY3) 

DSIN(RLRZ3) * DSIN(YWRX3) 

DRCSX(3) = DSIN(RLRZ3) * DSIN(PTRY3) * DCOS (YWRX3) - . . . 
DCOS(RLRZ3) * DSIN(YWRX3) 

DRCSZ(3) = DCOS(PTRY3) * DCOS(YWRX3) 

GET THE ANGLES AS RADIANS 



DRCRAX< 

DRCRAY< 

DRCRAZ< 



3) = DACOS( 
3) = DACOSI 
3) = DACOSI 



DRCSXI 

DRCSY 

DRCSZ 



CONVERT THE DIRECTION COSINES TO DEGREES 

DRCANX(3) = DACOS(DRCSX(3)) * RADEG 

DRCANY(3) = DACOS(DRCSY(3)) * RADEG 

DRCANZ(3) = DACOS(DRCSZ(3)) * RADEG 

FIND ANGLE BETWEEN LINK 2 AND LINK 3 TO CHECK IF THE ARM LINKS 
are PASSING THROUGH THE SINGULAR POINTS 



ANG23X = DRCANXI 


[2; 


1 - DRCANXI 


;3) 


ANG23Y = DRCANYI 


2 


1 - DRCANYI 


3 


ANG23Z = DRCANZI 


.2; 


1 - DRCANZI 


13) 


ANG12X = DRCANXI 


[1] 


1 - DRCANXI 


12) 


ANG12Y = DRCANYI 




1 - DRCANYI 


2) 


ANG12Z = DRCANZI 




1 - DRCANZI 


U) 



FIND THE JOINT LOCATION 

JX2 = JXl + 

JY2 = JYl + 

JZ2 = JZl + 

TIPX = JX2 + 

TIPY = JY2 + 

TIPZ = JZ2 + 




* DCOS(DRCRAX(2] 

* DCOS(DRCRAY(2 

* DCOS(DRCRAZ(2! 

* DCOS(DRCRAX(3; 

* DCOS(DRCRAY(3 

* DCOS(DRCRAZ(3 



END 

STOP 

FORTRAN 



SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 

SUBROUTINE CPROD ( VECTA , VECTB , MI , M J , MK ) 

IMPLICIT REALMS (A-Z) 

DIMENSION VECTA(3) ,VECTB(3) 

MI = VECTA (2) * VECTB (3) - VECTA 

MJ = VECTA(3) * VECTB (1) - VECTA 

MK = VECTA(l) * VECTB (2) - VECTA 

RETURN 
END 




VECTB! 

VECTB! 

VECTB! 
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